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ABSTRACT 


A  problem  of  motion  control  in  robot  motion  planning  is  to  find  a  smooth 
transition  while  going  from  one  path  to  another. 

The  key  concept  of  our  theory  is  the  steering  function,  used  to  manipulate 
the  motion  of  our  vehicle.  The  steering  function  determines  the  robot’s  position  and 
orientation  by  controlling  path  curvature  and  speed. 

We  also  present  the  -  neutral  switching  method  -  algorithm  that  provides  the 
autonomous  vehicle  with  the  capability  to  determine  the  best  leaving  point  which 
allows  for  a  smooth  transition  from  one  path  to  another  in  a  model-based  polygonal 
world. 

The  above  mentioned  algorithm  is  thoroughly  presented,  analyzed,  and  pro¬ 
grammed  on  a  Unix  workstation,  and  on  the  autonomous  mobile  robot  Yamabico. 
The  research  data  indicate  that  neutral  switching  method  improved  the  transition 
results  for  polygon  tracking,  star  tracking  motion,  and  circle  tracking.  Moreover, 
neutral  switching  method  enhances  robot  control  and  provides  a  more  stable  transi¬ 
tion  between  paths  than  any  previously  known  algorithm. 


VI 


TABLE  OF  CONTENTS 


I.  INTRODUCTION .  1 

A.  BACKGROUND  AND  MOTIVATION .  1 

B.  PROBLEM  STATEMENT .  1 

1.  Concepts  Definitions  and  Terminology .  2 

2.  Problem  Description .  3 

a.  Path  Classes .  3 

b.  Path  Classes  and  how  we  represent  them .  4 

C.  REVIEW  OF  RELATED  WORK .  5 

1.  Potential  Field  Methods .  6 

2.  Roadmap  and  Cell  Decomposition  Methods .  6 

3.  Polygon  Tracking  using  Images .  8 

D.  ASSUMPTIONS  AND  CONSTRAINTS .  10 

II.  PATH  TRACKING  METHOD .  13 

A.  THE  CONCEPT  OF  THE  STEERING  FUNCTION .  13 

B.  TRANSFORMATIONS .  13 

1.  Steering  Function  Method .  15 

C.  NEUTRAL  SWITCHING  METHOD .  17 

1.  Intersection  of  two  lines .  19 

2.  Problem  Statement .  21 

3.  Conclusions .  25 

D.  SIMULATION  RESULTS  .  26 

III.  POLYGONS  .  31 

A.  INTRODUCTION .  31 

1.  General  Definitions .  31 

2.  Problem  Definition .  33 

B.  SUMMARY .  37 

vii 


IV.  HARDWARE  AND  SOFTWARE  ARCHITECTURE  OF  YAMABICO- 


11 . 39 

A.  HARDWARE  SYSTEM .  39 

1.  CPU .  39 

2.  Wheels .  41 

3.  Sonars .  43 

B.  SOFTWARE  ARCHITECTURE  OF  YAM ABICO- 11 .  43 

1.  User  Program  Utility .  44 

2.  Library  Functions .  44 

3.  Functions  .  45 

4.  Odometry  Capability  . .  45 

V.  CON CLU SIONS-FUTURE  RESEARCH .  47 

A.  SUMMARY .  47 

B.  FUTURE  RESEARCH .  48 

APPENDIX .  51 

LIST  OF  REFERENCES  . 103 

INITIAL  DISTRIBUTION  LIST  . 105 


Vlll 


LIST  OF  FIGURES 


1.  Robot’s  world  space .  2 

2.  Motion  planning  problem .  4 

3.  A  world  and  paths  .  5 

4.  Visibility  graph .  7 

5.  Image  on  object .  8 

6.  Images  on  world .  9 

7.  Visibility  from  point  p  to  convex  polygon  B  (I)  .  10 

8.  Image  type  .  10 

9.  Representation  of  a  robot  in  a  global  coordinate  system .  11 

10.  Robot’s  relative  transformation .  14 

11.  Effect  of  smoothness  in  line  tracking  .  17 

12.  Tracking  of  X-axis  using  the  neutral  switching  method .  19 

13.  Tracking  of  X-axis  using  the  neutral  switching  method  from  dif¬ 
ferent  angles .  20 

14.  Intersection  of  two  lines  .  20 

15.  Intersection  of  two  lines  in  the  general  case .  22 

16.  Case  where  k  >  0 . 24 

17.  Case  where  k  <  0 .  25 

18.  Case  where  $  =  (#2  —  #i)  =  ±tt .  25 

19.  Case  where  $  =  (02  —  <?i)  =  0 .  26 

20.  Tracking  a  target  line  using  Neutral  switching  method .  27 

21.  Line  tracking  using  Neutral  switching  method .  28 

22.  Circle  tracking  using  the  neutral  switching  method .  28 

23.  Star  motion  using  the  neutral  switching  method  (I) .  29 

24.  Star  motion  using  the  neutral  switching  method  (II) .  30 

25.  Examples  of  simple  and  non-simple  polygons  (I) .  32 


W3C  QUALITY  mSPZUTSD  3 


26.  Orientation  of  an  edge .  32 

27.  Interior  and  exterior  angle  of  a  simple  polygon .  33 

28.  Convex  polygon .  33 

29.  Concave  polygon .  34 

30.  Representation  of  a  world  data  structure .  34 

31.  Pointer  manipulation  for  deletion  of  a  node  .  34 

32.  Pointer  manipulation  for  insertion  of  a  node .  35 

33.  A  safety  path  around  a  polygon .  35 

34.  Pseudocode  for  Tracking  a  line  using  the  x*  distance  .  36 

35.  Tracking  the  edges  of  a  polygon .  37 

36.  Pseudocode  for  Tracking  a  line  using  steer ()  function .  37 

37.  Tracking  of  four  given  lines .  38 

38.  Polygon  tracking  by  using  the  neutral  switching  method  ....  38 

39.  Diagram  of  Yamabico-11  hardware  architecture  . .  39 

40.  Autonomous  mobile  robot,  Yamabico-11 .  42 

41.  Yamabico-11  sonar  configuration .  43 

42.  MML-11  software  conceptual  architecture .  44 


x 


LIST  OF  TABLES 


I.  Values  for  speed  v,  and  smoothness  a  used  in  re<il  time  imple¬ 
mentation  . 


27 


xi 


xii 


I.  INTRODUCTION 

A.  BACKGROUND  AND  MOTIVATION 

One  of  the  most  challenging  problems  man  has  ever  faced  and  one  of  his  ulti¬ 
mate  goals  was  the  creation  of  autonomous  robots.  By  that  we  mean  that  the  robots 
will  be  able  to  be  programmed  by  high-level  programming  languages  and  will  be  able 
to  solve  a  variety  of  problems  from  a  variety  of  fields.  Hazardous  material  handling, 
welding,  painting,  and  assembly  in  factories  would  be  some  of  their  tasks.  In  addition, 
it  will  be  expected  that  autonomous  robots  will  be  able  to  perform  more  complex  jobs, 
such  as  mine  searching  or  fire  fighting.  The  tremendous  progress  in  microelectronics 
and  software  industry  as  well  as  the  development  of  related  technologies  gives  us  the 
ability  and  the  power  we  need  to  build  an  complete  autonomous  robot. 

The  robotics  and  automation  community  is  being  swept  by  broad,  pervasive 
technological  demands.  Successful  deployment  of  automomous  and  reprogrammable 
robots  is  expanding  the  technology  while  theory  and  implementation  continue  to 
advance  rapidly. 

Obstacle  avoidance  and  motion  planning  in  general  are  the  most  common  prob¬ 
lems  a  robot  is  encountered  by  acting  directly  on  the  world.  In  addition  autonomous 
robots  are  fundamentally  multidisciplinary,  incorporating  technologies  from  mechan¬ 
ical  and  electrical  engineering,  control  theory,  computer  vision,  estimation  theory, 
artificial  intelligence,  operations  research,  programming  languages,  mathematics  and 
physics.  Consequently,  a  vast  amount  of  disciplines  are  directly  or  indirectly  relevant 
to  mobile-robot  research. 

B.  PROBLEM  STATEMENT 

Almost  all  of  the  above  tasks  depend  upon  our  deep  understanding  of  the 
motion  problem.  In  order  the  robot  to  perform  even  the  simplest  task,  a  complicated 
combination  of  movements  is  needed.  In  addition  it  is  expected  to  move  safely  in 
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environments  filled  with  various  objects.  So  as  we  see  the  expectation  from  a  robot 
to  move  itself  safely  in  an  environment,  make  us  deal  with  the  problem  called  motion 
planning  problem.  Figure  1  shows  an  example  of  an  environment  our  robot  is  going 
to  perform  various  tasks  in. 


Figure  1.  Robot’s  world  space 


Various  techniques  have  been  developed  recently  dealing  with  the  motion  plan¬ 
ning  problem  and  a  lot  of  progress  has  been  made  during  the  last  several  years. 

One  of  the  most  revolutionary  theories  is  that  of  Professor  Yataka  Kanayama 
at  NPS.  This  thesis  concentrates  on  the  algorithms  presented  in  his  theory  which  are 
trying  to  approach  the  motion  planning  problem. 

1.  Concepts  Definitions  and  Terminology 

The  general  motion  planning  problem  for  an  autonomous  vehicle  can  be  stated 
as  follows:  Given  (1)  an  initial  state  of  the  vehicles,  (2)  a  desired  final  state  of  the 
vehicle,  and  (3)  any  constraints  on  allowable  motions,  find  a  collision-free  motion  of 
the  vehicles  from  the  initial  state  to  the  final  state  that  satisfies  the  constraints.  The 
above  standard  motion  planning  problem  is  extended  and  generalized  in  ways  which 
give  us  the  possibility  to  deal  with  environments  which  are  not  unknown  to  the  robot 
but  also  dynamically  changing. 

Another  definition  of  the  motion  planning  problem  can  be  given  by  the  follow¬ 
ing  schema: 
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“Given  a  robot  B,  an  environment  E,  and  a  motion  objective  0,  find  a  motion 
7r  for  B  amidst  E  that  achieves  0  subject  to  some  optimality  criterion  C(or  report 
that  no  motion  exists). (Chee-Keng  Yap). 

Another  idea  that  plays  a  major  role  in  our  research  is  that  of  localization. 
What  we  mean  by  that  is  the  exact  determination  of  the  current  position  of  the  robot 
and  its  orientation. 

One  of  the  most  fundamental  concepts  behind  every  motion  planning  theory 
is  that  one  local  motion  planning.  What  we  mean  by  that  is  finding  the  best  motion 
among  a  path  class  using  a  local  motion  control  algorithm.  In  our  case  we  axe  going 
to  present  the  steering  function  algorithm  in  details. 

2.  Problem  Description 

The  motion  planning  problem  is  a  very  fundamental  one  in  robotics-even 
though  one  of  the  most  complex  ones-and  as  we  told  before  its  main  purpose  is  to 
enhance  the  robot  with  the  capability  to  generate  its  own  motion.  One  of  the  parts 
of  this  problem  is  called  local  motion  control  and  we  can  see  it  in  the  next  Figure  2. 

A  concept  very  much  related  to  our  work  is  that  of  “path  classes”,  and  af¬ 
ter  giving  the  necessary  definitions  and  expanations  of  this  concept  we  are  going  to 
proceed  to  the  description  of  the  problem  we  deal  with  in  this  thesis. 

a.  Path  Classes 

This  subsection  defines  a  list  of  concepts  about  path  classes  and  their 
importance  to  our  problem. 

A  path  f  in  a  world  W  is  a  continuous  function 

/  :  [0, 1]  -»  /ree(W) 

with  /( 0)  ^  /( 1).  We  consider  a  path  /  to  be  a  directed  curve  with  natural  direction 
from  /( 0)  to  /( 1).  The  two  points  /( 0)  and  /( 1)  are  called  its  endpoints  and  we  say 
that  the  path  joins  them.  We  usually  denote  /( 0)  as  a  start  S  and  /( 1)  as  a  goal  G. 
We  assume  that  /  is  rectifiable(its  length  is  finite).  A  world  of  three  ccw  polygons 
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Figure  2.  Motion  planning  problem 


B\,  B2  and  B3,  one  cw  polygon  Bo,  and  the  paths  from  S  to  G  can  be  depicted  in 
Figure  3. 

b.  Path  Classes  and  how  we  represent  them 
Given  a  starting  point  S  and  a  goal  point  G  in  a  polygonal  world  W, 
our  effort  is  to  find  a  continuous,  smooth  path  that  connects  the  start  configuration, 
S,  to  the  goal  configuration,  G.  In  Figure  3  for  example,  we  can  see  three  different 
path  classes.  We  are  using  the  notion  of  directed  v-edges  to  represent  each  path 
class.  In  its  most  general  form,  a  path  class,  p,  that  includes  a  path  /  is  symbolically 
represented  by  a  path  sequence.  For  instance,  the  path  classes  f\  and  fi  in  Figure  3 
are  represented  by: 

fi  =  [B3/Bo][B2/Bo}[B1/B0] 

h  =  [BsIBoWBsIBtWBsjBtWBolBt] 
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Figure  3.  A  world  and  paths 

To  find  the  best  path  class  we  have  to  take  into  consideration  the 
“weight”  of  each  edge  related  to  the  cost  function.  This  best  path  class  we’ve  just 
found  and  the  task  we  want  our  robot  to  execute  affect  the  steering  function  and  its 
desired  motion. 

As  we  see  the  objective  of  using  path  classes  is  to  provide  useful  in¬ 
formation  for  the  local  motion  control  problem.  In  our  research  we  are  going  to 
investigate  the  safe  navigation  of  an  autonomous  vehicle  through  an  environment, 
using  the  steering  function  and  the  Neutral  Switching  method,  to  achieve  smoothness 
of  the  motion. 

C.  REVIEW  OF  RELATED  WORK 

During  the  last  years  various  techniques  have  been  used  trying  to  solve  robot- 
motion-planning  problems.  We’re  are  going  to  review  some  of  them  here.  Figure  3  is 
an  example  of  the  different  paths  a  robot  would  choose  to  approach  a  given  goal,  and 
gives  us  the  central  idea  of  the  problem  we’re  dealing  with. 

We  are  going  to  say  a  few  words  about  the  different  approaches  to  the  motion 
planning  problem  but  we’re  going  to  focus  our  attention  more  to  the  last  previous 
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method  used  for  polygon  tracking.  This  way  the  reader  would  have  a  very  good  back¬ 
ground  in  understanding  the  problem  and  he  would  be  able  to  see  the  advantages  of 
the  new  method  Neutral  Switching  Method  and  its  simplicity.  There  are  three  “clas¬ 
sical”  approaches  to  motion  planning:  roadmap  methods, cell  decomposition  methods , 
and  potential  field  methods.We  will  also  describe  the  concept  of  an  image  and  we  will 
show  how  this  notion  was  used  for  polygon  tracking  motion. 

1.  Potential  Field  Methods 

Potential  Field  Methods  can  be  very  efficient.  They  are  usually  called  local 
methods  while  the  roadmap  and  the  cell  decomposition  are  called  global  methods. This 
method  consists  of  defining  a  potential  field  which  is  represented  by  a  function  /  : 
W  — >  1Z  and  it  is  a  combination  of  attractive  and  repulsive  potentials.  Attractive 
potentials  tend  to  pull  the  robot  towards  the  goal  while  at  the  same  time  repulsive 
potentials  push  the  robot  away  from  them.  The  negated  gradient  of  the  total  potential 
over  the  free  space  is  treated  as  an  artificial  force  applied  to  our  robot.  The  direction 
of  this  force  shows  the  direction  of  the  motion  the  robot  should  follow. 

The  main  disadvantage  of  these  methods  is  since  these  methods  are  essentially 
fastest  descent  optimization  methods,  the  robot  can  become  trapped  in  a  local  minima 
of  the  potential  field.  Some  of  the  approaches  used  to  avoid  this  problem  is  to  design 
the  potential  functions  in  a  way  that  they  have  no  local  minima,  or  to  complement 
the  basic  potential  field  approach  with  such  powerful  mechanisms  which  give  us  the 
capability  to  escape  from  local  minima. 

2.  Roadmap  and  Cell  Decomposition  methods 

These  two  methods  generally  include  an  initial  processing  step  aimed  at  cap¬ 
turing  the  connectivity  of  the  free  space  in  concise  representation.  As  we  mentioned 
above  these  methods  are  called  global  methods. 

The  roadmap  method  for  example  consists  of  capturing  the  connectivity  of  the 
robot’s  free  space  in  a  network  of  one-dimensional  curves,  called  the  roadmap ,  lying 
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in  the  free  space.  It  is  also  called  skeleton  approach  and  how  it  actually  works  is  that 
after  a  roadmap  p  has  been  constructed  the  path  planning  is  reduced  to  connecting 
the  start  and  the  goal  configurations  to  />,  and  searching  p  for  a  path. 


Figure  4.  Visibility  graph 


Many  other  methods  have  been  proposed  based  on  this  general  idea.  They 
include  visibility  graph ,  Voronoi  diagram ,  freeway  net ,  silhouette  and  retraction. 

Cell  decomposition  methods  are  the  most  widely  studied  and  implemented 
ones.  The  principle  that  lies  behind  them  is  that  of  decomposing  the  robot’s  free 
space  into  simple  regions,  called  cells.  The  path  planning  is  then  performed  by  finding 
a  path  in  G  from  the  node  corresponding  to  the  start  cell  (the  cell  containing  the  start 
configuration)  to  the  node  corresponding  to  the  goal  cell  (the  cell  containing  the  goal 
configuration). 

Both  methods  consist  of  constructing  a  global  data  structure  that  can  later 
be  used  for  solving  one  or  more  motion  planning  problems.  There  are  two  serious 
problems  though: 

1.  The  computations  of  the  data  structures  tend  to  be  very  expensive  in  both 
time  and  memory,  and 
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2.  They  do  not  seem  to  be  suitable  for  robots  with  non-holonomic  constraints 
such  as  car-like  robots  or  multi-body  mobile  robots. 

Our  readers  will  be  able  to  find  a  thorough  discussion  of  these  approaches  in  [9]. 

3.  Polygon  Tracking  using  Images 

Given  an  edge  L  and  a  point  p,  the  image,  im(p,L),  of  p  on  L  is  defined 
as  the  closest  point  on  L  from  p.  The  distance  d(p,L)  from  p  to  L  is  defined  as 
d(p,L)=d(p,im(p,L)).  We  assume  that  in  our  world  two  distinct  points  p1  =  (zi,J/i) 
and  p2  =  (or2,  J/2)  are  given.  The  Euclidean  distance  d(p1,p2)  between  them  is  defined 
as: 

d(puP2)  =  y/(x  1  -  x2)2  +  (j/1  -  t/2)2  (I-1) 


Figure  5.  Image  on  object 

Assume  now  that  there  is  an  object  o  in  a  plane.  An  object  might  be  the  form 
of  a  point,  a  line,  an  open  line  segment,  a  polygon,  or  other  set  of  points.  We  define 
the  shortest  distance  d(p,  o)  between  a  point  p  and  an  object  o  as  follows: 

d(p,o )  =  mind(p,pi)  (1-2) 

Pi  Go 

Eq.  1.2  generalizes  the  function  d  defined  by  Eq.  1.1. 
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Definition:  An  image  im(p,B)of  a  point  p  G  free(B)on  a  polygon  B  is  the  closest  point 
from  p  on  B. 

The  image  is  a  vertex  on  B  or  a  point  on  an  open  edge  e  in  B.  See  (Figure  5). 

If  a  world  W  has  more  than  one  object,  an  image  im(p,  W)  is  defined  as  the 
image  im(p,Oi )  such  that  d(p,Oi )  is  the  minimum  over  all  objects  in  W  (Figure  6). 


Given  the  images  in  a  world  we  have  to  solve  the  following  problem:  given 
a  point  p  in  free  space  and  a  convex  polygon  B ,  determine  whether  the  image  from 
p  to  B  is  on  an  edge  or  on  a  vertex  of  B.  The  following  definitions  are  useful  in 
understanding  the  concepts  used  in  this  method  for  polygon  tracking: 


Assume  now  that  we  are  given  a  convex  polygon  B  =  (uj,  •  •  • ,  vn)  and  a  point 
p  G  free(B).  The  significant  notion  for  our  purpose  is  the  following  classification  of 
each  vertex  of  B  with  respect  to  the  segment  pvt .  Each  vertex  of  B  is  said  to 
be  visible ,  invisible ,  cw-tangential ,  or  ccw-tangential  (we  should  add  with  respect  to 
segment  pvi,  but  we  shall  normally  imply  this  qualification)  (see  Figure  7). 

Definition:  Let  B  be  a  convex  polygon,  and  let  a  point  p  G  free(B). 
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ccw-tangential 


Figure  7.  Visibility  from  point  p  to  convex  polygon  B  (I) 


Convex  Polygon  B 

Point  p 


Find  Convex  Image 

Algorithm 

m  ~ 

Image  Type  (Edge  or  Vertex) 
Vertex  v 

Orientation  from  p  to  image 
Closed  Distance 


Figure  8.  Image  type 


•  A  vertex  U;  is  tangential  from  point  p  if  the  two  vertices  adjacent  to  V{  lie  on 
the  same  side  of  the  line  containing  pvj. 

•  A  vertex  Vi  is  visible  if  the  segment  pvl  does  not  intersect  the  interior  of  B  and 
the  two  vertices  adjacent  to  v,  lie  on  opposite  sides  of  the  line  containing  pvt. 

•  A  vertex  Vi  is  invisible  if  the  segment  pv{  intersects  the  interior  of  B. 

D.  ASSUMPTIONS  AND  CONSTRAINTS 

The  problem  of  finding  the  optimal  path  for  the  continuous  motion  of  a  given 
vehicle  from  a  given  initial  configuration  to  a  desired  final  goal,  is  definitely  subject  to 
certain  geometric  constraints  during  its  motion.  These  constraints  do  not  permit  the 
body  of  the  vehicle  to  come  in  contact  with  certain  obstacles  or  ’walls’,  of  the  given 
environment.  This  way  issues  related  to  the  mechanical  interaction  are  avoided. 
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Also  our  robot  has  a  fixed  number  of  degrees  of  freedom.  As  we  see  in  the 
next  figure  9  our  vehicle  has  three  degrees  of  freedom  when  moving  on  a  flat  surface. 
Precisely  what  we  mean  by  this  is  the  following  :  relative  to  some  global  coordinate 
system,  the  robot  can  be  at  any  position  specified  by  two  coordinates,  x  and  y,  and 
pointed  in  any  direction  specified  by  a  third  coordinate  angle  0.  These  three  degrees 
of  freedom  (x,y,0)  give  us  the  distance  to  and  the  angle  between  the  global  frame, 
and  a  local  reference  frame,  R,  on  the  robot.  (We  could  have  put  the  frame  anywhere 
on  the  robot  but  because  the  robot’s  center  of  rotation,  is  the  point  midway  between 
its  two  drive  wheels,  we  chose  that  point). 


Figure  9.  Representation  of  a  robot  in  a  global  coordinate  system 

Also  in  the  theory  of  Professor  Kanayama  we  avoid  any  mathematical  methods 
which  are  coordinate  system- sensitive.  For  instance,  a  curve  is  never  represented  in 
a  form  of  y=f(x).  Most  important  of  all  according  to  this  theory  paths,  motions  and 
environments  of  a  robot  are  treated  as  continuous  entities  rather  than  discrete  ones. 
This  policy  causes  some  problems  sometimes  but  make  real  applications  easier. 

Also,  we  believe  that  moving  objects  in  general  are  handled  better  if  direction 
is  attached.  So  all  the  geometrical  objects  like  lines,  circles  and  curves  have  directions 
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(forward  or  backward).  We  believe  also  that  this  approach  allows  a  deeper  study  of 
the  inherent  mathematical  structure  of  the  different  problems. 
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II.  PATH  TRACKING  METHOD 

A.  THE  CONCEPT  OF  THE  STEERING  FUNCTION 

In  this  chapter  we  introduce  the  mathematical  framework  that  is  used  in  our 
theory  and  in  paxticular  we’re  going  to  discuss  the  steering  function  in  details.  We’re 
going  to  show  how  this  function  is  used  to  manipulate  the  motion  of  our  vehicle,  how 
it  is  related  to  the  robot’s  position  and  orientation  and  how  it  affects  the  movement 
of  our  robot  in  general.  It  is  also  necessary  at  this  point,  to  give  to  the  reader  some 
important  definitions  we  are  using  through  out  the  proposed  theory  of  Professor 
Kanayama. 

B.  TRANSFORMATIONS 

Let  7 Z  denote  the  set  of  all  real  numbers. 

Definition:  A  transformation ,  q ,  is  defined  by 


where  x,y,6  £  11- 

The  set  of  all  transformations  is  denoted  by  T.  For  example,  (3,  1,tt/3)t  £ 
T.  Obviously,  a  transformation  q  is  interpreted  as  a  two  dimensional  coordinate 
transformation  from  the  global  Cartesian  coordinate  system  3  to  another  coordinate 
system  T . 

Definition:  The  transformation  group  (T,  o)  consists  of  the  set  T  of  transformations, 
where 

T  =  {(x,y,0)T\x,y,8  £  7 1} 

and  the~binary  operator  ( composition  function ),  o,  is  defined  as  follows: 
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Let  qi  =  (*i,yi,0i)T,  q2  =  (x2,y2,02)T  €  {T ,  o),  then 


qioq2 


( 


\ 


X\  +  X2  cos  Oi  —  2/2  sin  0\ 
2/i  +  22  sin  0i  +  2/2  COS  0i 

01  4"  02 


\ 

/ 


The  interpretation  of  q\  o  cj2  in  the  domain  of  two-dimensional  coordinate  transfor¬ 
mations,  is  the  composition  of  the  coordinate  transformations  q\  and  92 ■ 


Definition :  The  inverse  q~l  of  a  given  transformation  q  =  (x,y,0)T  is  defined  as: 


— x  cos  0  —  2/sin0 
x  sin  0  —  y  cos  0 


V 


Another  poerful  tool  in  this  transformation  theory  is  the  concept  of  relative 
transformations.  We  already  know  that  a  vehicle’s  configuration  is  represented  by 


Figure  10.  Robot’s  relative  transformation 
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(Ill) 


?V  =  (x»,S/v,0v)T 

This  transformation  represents  the  three  degrees  of  freedom  a  rigid  body  possesses  in 
a  place. 

The  relative  transformation  now  is  defined  as 

9v  =(**,! (II.2) 

of  the  object  with  respect  to  the  vehicle  ’s  coordinate  system.There  exists  also  a 
relation  qv  o  q*  =  q0  between  <?v,<?o  and  q*. 

Proposition  II.  1  If  the  transformations  of  a  vehicle  and  an  object  are 

qv  =  (xvjJ/vi^v)  and  qo  =  (xo^J/o^o)  (II. 3) 

the  relative  transformation  q*  of  the  object  with  respect  to  the  vehicle  is 
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*  _ 


<  ( x0  —  iv)  cos  By  +  (y0  —  J/v)  sin  0V  ^ 
(-x0  -  xv)  sin 0V  +  (Vo  -  J/v)  cos  0V 

\  @o  —  ) 


1.  Steering  Function  Method 

We  know  already  that  an  ordinary  vehicle  has  two  control  variables:curvature 
At  and  speed  v.  The  assumption  we  make  in  our  theory  is  that  each  vehicle  operates 
at  a  relatively  low  speed  and  that  speed  is  proportinal  to  the  curvature  k.  We  also 
take  into  consideration  that  our  robot’s  rotational  rate  u>  is  proportional  to  its  path 
curvature  k  if  its  speed  v  is  constant. 


U  =  K  V  (11*4) 

One  of  the  assumptions  we  made  in  our  introduction  was  that  a  vehicle  s 
heading  direction  and  curvature  must  be  continuous.  So  we  have  to  find  a  way  to 
control  the  curvature  k  of  our  robot. 

In  order  to  continuously  change  k  we  should  compute  Therefore  we  intro¬ 
duce  and  adopt  through  our  whole  study  the  general  form  of  the  steering  function 
which  we  have  tested  and  it  works  perfectly. 


15 


—  =  -(aAK  +  bAe  +  cAd) 
ds 

=  — (a(/c  —  Kd)  +  b(0  —  $d)  +  cAd), 


(n.5) 


where  a, 6,  and  c  are  positive  constants,  and  A d  is  the  “signed’  distance  between  the 
vehicle  and  a  directed  line.  Also,  k  is  the  path  curvature,  6  the  vehicle  s  heading(which 
is  equal  to  the  path  tangential  direction),  Kd  the  desired  curvature,  and  6d  the  desired 
heading  direction.  Ad  is  positive  (Ad  >  0)  or  negative  (Ad  <  0)if  the  robot  is  on  the 
left  or  the  right  side  of  the  reference  path  respectively. 

A  very  important  factor  that  influences  the  efficiency  of  the  steering  function 
formula  is  the  correct  choosing  of  the  three  constants  (a,  6,  c).  The  proper  track¬ 
ing  of  our  reference  path  depends  heavily  on  the  correct  combination  of  these  three 
constants. 

Professor  Kanayama  in  his  theory  had  linearized  the  system  for  computing 
these  constants  and  after  solving  the  equation 

A3  +  aA2  +  BX  +  c  =  0  (H.6) 

he  found  the  eigenvalues  A  that  satisfy  that  equation. 

To  obtain  the  proper  condition  on  the  coefficients  for  achieving  asymptotic 
stability, Professor  Kanayama  had  to  refer  to  Routh-Hurwitz  criterion  which  says  that 
a,  6,  and  c  must  be  all  positive  and  ab  —  c  >  0.  According  to  this  we  have 


a 

=  K\  +  K2  +  ^3 

(H.7) 

p 

=  K2&2>  ~l“  K$K\  +  K\K2 

(II.8) 

c 

=  K1K2K2, 

(H.9) 

and  taking  all  the  negative  real 

eigenvalues  equal, 

a  =  3  k 

(11.10) 

13  =  3  k2 

(11.11) 

- 

C  =  K3 

(11.12) 
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for  some  positive  constant  k. 

At  this  point  we  have  to  say  a  few  words  about  smoothness  a  and  the  important 
role  it  plays  in  the  steering  function. 

Smoothness  is  very  essential  for  our  robot’s  navigation  and  motion  planning 
in  general  because  an  unproper  value  of  it  could  cause  slippage  of  the  wheels,  or  could 
cause  a  very  unsafe  motion  for  the  vehicle  . 


We  define 


(11.13) 


where  k  is  the  curvature. 

In  general  large  smoothness  a  results  in  faster  motion  and  smaller  smoothness 
in  a  slower  one.  Choosing  a  value  for  smoothness  ct  depends  on  the  environment  our 
vehicle  is  going  to  move  in.  In  an  environment  with  many  obstacles  we  need  to  use 
a  smaller  smoothness  in  order  to  avoid  the  obstacles  while  in  the  case  we  want  to 
move  faster  we  have  to  use  larger  smoothness.  The  next  figure  gives  us  an  idea  of 
how  smoothness  affects  line  tracing  simulation  results(using  the  old  method). 


y 


Figure  11.  Effect  of  smoothness  in  line  tracking 


C.  NEUTRAL  SWITCHING  METHOD 

The  revolutionary  method  of  neutral  switching  proposed  by  Prof.Kanayama  is 
explained  in  this  section.  We  will  also  show  here  our  simulation  results  obtained  by 
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working  on  Sun/Unix  workstation  in  the  NPS  Artificial  Intelligence  Lab. 

To  ensure  that  our  robot  has  smooth  motion  behavior,  while  transitioning 
from  one  path  to  another  we  have  to  find  the  best  point  the  robot  has  to  leave  its 
path  it  currently  is  moving  on,  and  proceed  to  the  next  path.  So  the  key  notion 
behind  Neutral  Switcing  Method  is  selecting  an  appropriate  leaving  point  from  the 
current  path  segment  our  vehicle  is  moving  on  to  the  next  one.  The  method  we  ’re 
using  gives  us  a  unique  point  for  each  path  combination. 

An  early  leaving  from  the  current  path  would  have  as  a  result  an  intersection 
with  the  next  path  something  we  really  want  to  avoid  while  a  late  leaving  from  the 
current  path  would  have  as  a  result  a  strange  “turning  back”  behavior  away  from  the 
next  path.  Both  cases  are  undesirable  and  the  way  to  avoid  them  is  to  select  a  point 
where  the  steering  function  returns  a  “zero  feedback”  value: 

ak 

—  =  -{aAK  +  bAO  +  cAd)  =  0 
ds 

For  the  beginning  we  apply  this  principle  to  a  line-to-line  switching  and  more 
specifically  from  an  Y  axis  to  the  X  axis.  When  the  vehicle  is  traveling  on  the  Y  axis 
down,  its  curvature  is  0  and  its  orientation  is  equal  to  —  j.  So  we  obtain 

=  —  (oA/c  +  bA6  +  cAd)  =  (&(— — )  +  cy]  =  0 
ds  '  z  ' 


Therefore 


b  7r  3k2  7r  _  37r  _  37 rcr 

c  2  =  "P‘2  =  2k=~2~ 


4.712  <r 


(11.14) 


The  results  obtained  by  our  simulation  program  for  this  case  are  clear  to  the  next 


figure  12  and  show  the  effectiveness  of  this  method.  Also  we  are  going  to  present 
simulation  results  and  how  this  method  was  used  for  polygon  tracking  and  star  track¬ 


ing.  (The  reader  will  be  able  to  find  all  the  necessary  codes  for  finding  the  leaving 
point  for  the  various  cases  in  the  Appendix  of  this  Thesis). 
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Figure  12.  Tracking  of  X-axis  using  the  neutral  switching  method 


Figure  13  shows  the  simulation  results,  when  the  vehicle  is  tracking  the  X-axis 
from  different  angles:30  degrees,  60  degrees,  90  degrees,  120  degrees  and  150  degrees. 
The  initial  configuration  is  qo  =  ((0.0, 0.0),  —  7r/2,0). 

As  we  see,  all  the  above  results  were  obtained  for  special  cases  where  the 
vehicle  is  moving  on  one  of  the  two  axis(X,  or  Y).  Now  we  are  going  to  examine  the 
more  general  case  where  the  two  lines-path  segments-  cross  each  other  randomly. 

1.  Intersection  of  two  lines 

We  consider  the  general  case  where  we  have  two  lines  with  configurations 

91  =  (*i,2/i,  0i,  «i)  (11.15) 

92  =  (*2,  i/2,  02,  *2)  (11.16) 
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Figure  13.  Tracking  of  X-axis  using  the  neutral  switching  method  from  different  angles 

respectively,  given  k-i  =  «2  =  0.  In  other  words  the  two  path  segments  are  straight 
lines. 

Let  the  intersection  of  the  two  lines  be  (x,y).  Then  it  is  obvious  that  the 
distance  between  this  point  and  either  of  the  lines  is  zero  (Figure  14).  The  calculation 
of  the  leaving  point  is  simple  and  straight  forward,  as  we  will  see  right  away. 


Figure  14.  Intersection  of  two  lines 

(y  -  J/i)cos#i  -  (x  —  Xi)sin0i  =  0  (H-17) 
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(y  -  2/2)  cos  O2  — (x  —  x2 )  sin  02 


0 


(11.18) 


This  is  rewritten  as 


Therefore,  if 

sin  6 1 
sin  02 


■  cos  0i 
-  cos  0 2 


■  xi  sin  0\  —  yi  cos  0\  ^ 
y  x2  sin  02  —  y2  cos  02  f 


=  sin  $2  cos  0i  —  cos  02  sin  0i  =  sin(02  —  #i)  7^  0 


we 


can  solve  the  simultaneous  equations  11.17  and  11.18  as  follows. 


1 

sin(02  —  #i) 


(xi  sin  —  j/i  cos 
(x2  sin  02  —  y2  cos  02) 


#i)  —  cos  0i 
cos  02 


I  - *  - 1 

cos  fl2(xi  sin  61  —  yi  cos  0i)  +  cos  #i(x2  sin  02  —  y2  cos  ^2) 
sin(02  —  0i) 


(11.19) 


(11.20) 


(11.21) 


1  sin  0\  (xi  sin  0i  —  2/1  cos  9i ) 

sin(02  —  #1)  sin02  (x2sin^2  —  y2cos02) 

sinfli(x2  sinfl2  —  2/2  cosfl2)  —  sinfl2(xi  sinfli  —  2/1  cos  0i) 

sin(02  —  #i) 


2.  Problem  Statement 

Suppose  that  we  have  two  lines  with  configurations 


(11.22) 


gi  =  (xi,yi,0i,Ki)  (11.23) 

q2  =  (x2, 2/2, 02,  *2)  (11.24) 

respectively. We  assume  that  in  our  case  the  curvature  values  for  both  lines  equal  zero. 

The  main  problem  then  is  focused  on  finding  the  point  pu  =  (xn,2/ii)  °n  <?i 
so  that  pn  will  become  the  leaving  point  for  neutral  switching.  From  this  point  an 
autonomous  mobile  vehicle  -under  the  nonholonomic  constraint-  will  use  to  track  the 
next  line.  Upon  competion  of  the  above  calculation  we  will  simulate  the  motion  of 
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Figure  15.  Intersection  of  two  lines  in  the  general  case 

the  robot  in  C  language.  The  value  sigma  ^-(smoothness)  will  be  given  by  the  user 
The  leaving  point  pu  =  {xiuVn)  must  satisfy  two  conditions: 

First  condition 

—  =  -(aA*  +  bA9  +  cAd)  =  0 
ds 

In  our  case 

k  =  0,  and  9  =  9\ 

Also  we  have 


A  A 


-bjOi  -  Ot)  _  mh _ £i!  _  3cr(^2  _  0X) 


(II.2I 


On  the  other  hand 

Ad  =  0,  where  (z*,y*)  (11.26) 

is  in  q'2 ’s  coordinate  system. 

Ad  =  y*  =  (x  —  x2)  sin  02  +  (y  —  y  2)  cos  92  (11.27) 

Second  condition 

The  point  (x,y)  must  be  on  the  first  directed  line.  On  the  other  hand  y*  =  0 
on  the  q\ ’s  coordinate  system. 

y*  =  (x  —  x  1)  sin  0i  +  (y  —  yi)  cos  0i  (11.28) 

From  (1),(2)  =» 

—  x  sin  92  +  y  cos  92  =  —x2  sin  92  +  y2  cos  92  +  3 <t(92  —  9 1)  (11.29) 

—  x  sin  9i  +  y  cos  9\  =  —X\  sin  9\  +  j/i  cos  9\  (11.30) 

From  (3), (4)  =^ 

—X\  sin  9\  +  yi  cos  9\ 

—x2  sin  92  +  y2  cos  92  +  3  cr(92  —  9\ ) 

The  next  condition  also  must  satisfy: 

D  =  sin^2  cos  —  cos  02  sin0i  =  sin(02  —  0i)  7^  0  (11.32) 

By  solving  the  equations  (11.31)  we  find  the  coordinates  of  the  point  in  question: 

1  (— x\  sin  0i  +  yi  cos0i)  +cos0i 

x  ——  - 

sin(02  —  0i )  (— 2:2  sin  02  +  y2cos92  +  3<t(02  —  0i)  +cos02 

cos  9^—xi  sin  0i  +  yi  cos0i)  —  cos  0i(— X2  sin  02  -fi  y <2  cos  02  +  3<t(02  —  0i) 

sin(02  —  0i) 

sin9i  (-xi  sin0i  +  t/i  cos0i) 

sin92  (—x2  sin  02  +  J/2  cos  02  +  3cr(02  —  0i) 

.sm0 i(—x2  sin  92  +  y 2  cos  023<t(02  —  0i))  +  sin  02(— x\  sin  0i  +  yi  cos  0i) 

sin(02  —  0i) 


1 

sin(02  —  0i ) 


(11.34) 


(11.33) 


sin9\  +  cos  0i  X 


sin02  +  cos  02  /  V  V 


23 


As  we  see,  the  equation  of  the  steerinf  function,  governs  the  behavior  of  the 
robot  and  manipulates  its  motion,  by  forcing  changes  in  its  position  and  orientation. 
The  most  interesting  feature  of  the  neutral  switching  method  is  that  is  based  also  on 
symmetry.  This  means  that  it  works  and  it  predicts  perfectly  the  behavior  of  the 
robot  in  all  the  cases. 

We  will  now  discuss  now  the  various  cases  we  will  encounter  in  the  real 
world. We’re  also  going  to  show,  the  way  this  symmetry  affects  each  one  of  the  inter¬ 
actions  of  the  robot  in  the  real  world.  In  other  words  the  symmetry  that  exists  in 
nature  interchanges  the  terms  within  the  equation,  yet  leaves  the  equation  the  same. 

We  must  now  determine  the  different  cases  the  motion  of  our  robot  falls  in. 

Case  1.  This  case  is  depicted  very  clearly  in  Figure  16  and  is  the  case  where 
the  vehicle  should  turn  when  the  condition  >  0  is  fulfilled. 


Figure  16.  Case  where  k  >  0 


Case  2.  This  case  is  similar  to  case  1  with  the  difference  that  now  the  curvature 
k  is  less  than  zero.  This  means  that  the  vehicle  has  to  turn  when  the  condition  <  0 
is  fulfilled. 

Case  3.  If  $  =  (02  —  0\)  =  ±7r  we  have  the  case  that  is  depicted  in  Figure  18 
Case  4.1f  $(92  —  Oi)  =  0  then  either  L\  =  L2  or  the  two  lines  are  parallel  as 
we  can  see  in  Figure  19 
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Figure  17.  Case  where  k  <  0 


Figure  18.  Case  where  $  =  (02  —  #i)  =  ±?r 


3.  Conclusions 

From  our  discussion  so  far  the  reader  should  be  able  to  distinguish  neutral 
switching  method’s  two  essential  features: 

1.  symmetry 

2.  The  ability  to  explain  large  amounts  of  experimental  data  with  the  most  eco¬ 
nomical  mathematical  expressions. 
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Figure  19.  Case  where  <&  =  (82  —  $1)  =  0 

D.  SIMULATION  RESULTS 

Here  we  present  more  simulation  results  obtained  by  the  imlementation  in 
C  language  of  neutral  switching  method. All  the  codes  were  tested  in  UNIX/SUN 
workstation  at  the  AI  laboratory.  Those  results  show  clearly  the  practical  aspect  and 
the  efficiency  of  our  method. 

In  Figure  20,  the  vehicle  is  supposed  to  have  initial  configuration  q\  =  ((0.0, 20.0),  45.0, 0.0), 
and  the  target  line  q2  =  ((30.0,0.0), —45.0, 0.0).  The  value  of  smoothness,  a  =  10, 
while  for  Figure  21,  the  value  of  smoothness,  <7  =  40. 

The  example  in  Figure  22,  shows  the  result  of  the  trajectory  of  circular  track¬ 
ing.  The  center  of  the  circle  is  at  (0.0, 0.0),  the  radius  equals  10,  and  the  value  of 
smoothness,  a  =  10.  Our  vehicle’s  configuration  is  qv  =  ((0.0, —12.5),  0.0, 0.0). 

In  Figure  23,  and  Figure  24  we  can  see  simulation  results  for  “star”  tracking, 
for  smoothness,  sigma  =  5,  and  smoothness,  sigma  =  10  respectively. 

The  neutral  switching  method  algorithm  described  in  this  thesis  has  been 
impemented  in  MML,  and  tested  on  the  experimental  robot  Yamabico-11  in  AI  lab. 

We  tried  various  values  for  the  smoothness  a  and  the  speedu  of  the  robot.  The  robot 
started  with  initial  configuration  qo  =  ((0.0, 0.0),  0.0, 0.0)  and  was  supposed  to  track 
the  line-ft.  The  results  obtained  so  far  show  that  the  algorithm  is  working  well  and 
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Figure  20.  Tracking  a  target  line  using  Neutral  switching  method 


it  is  very  efficient  for  robot  motion  planning  and  motion  control. 


a 

speed  v 

4.712cr 

10 

30 

ft  =  ((47.12, 0.0),  tt/2,  0.0). 

20 

20 

ft  =  ((94.24, 0.0),  tt/2,  0.0). 

30 

10 

=  ((141.36, 0.0),  tt/2,  0.0). 

Table  I.  Values  for  speed  v,  and  smoothness  a  used  in  real  time  implementation 
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Figure  23.  Star  motion  using  the  neutral  switching  method  (I) 
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Figure  24.  Star  motion  using  the  neutral  switching  method  (II) 
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POLYGONS 


A.  INTRODUCTION 

The  purpose  of  this  chapter  is  to  present  the  various  concepts  and  definitions 
about  polygons,  and  polygonal  worlds.  We  cover  basic  concepts  which  form  the 
basis  of  our  theory  as  well  as  the  necessary  schemas(data  structures)  we  are  using 
to  represent  a  polygon.  After  that  we  use  the  neutral  switching  method  to  track  a 
polygon. 

1.  General  Definitions 

We  represent  a  polygon  by  an  ordered  set 

B  =  {ui,u2,  •  •  ■  ,vn},  n>  3 

of  n  distinct  points(vertices)  in  a  plane. 

As  we  see  from  the  above  definition  the  simplest  polygon  is  a  triangle,  since  a 
polygon  must  have  at  least  three  edges.  We  call  a  polygon  with  n  vertices  a  n-gon. 

The  union  of  all  edges  in  a  polygon  B  is  considered  to  be  the  value,  val(B), 
of  the  polygon. 

Definition :  A  polygon  is  called  simple  if  it  satisfies  the  following  two  condi¬ 
tions  (Kanayama) 

1.  if  no  triple  of  vertices  ( v,<p(v),<p2(v ))  in  B  are  colinear 

2.  if  there  is  no  pair  of  nonconsecutive  edges  sharing  a  point  in  it 

A  simple  polygon  partitions  the  plane  into  two  disjoint  regions,  the  bounded ,  and 
unbounded  ones,  separated  by  the  polygon  (Jordan  curve  theorem ). 

In  our  theory  we  make  the  assumption  that  the  first  vertex  in  a  polygon  B  is 
considered  that  with  the  minimum  ^-coordinate  among  all  the  vertices. 

The  direction  of  an  edge  u,  tp(v)  in  a  polygon  B  is  defined  as 

tf(pi,P2)  =  atan2(y2  -  yi,  x2  -  Xi) 
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simple 

simple 

not  simple 

(a) 

(b) 

(c) 

Figure  25.  Examples  of  simple  and  non-simple  polygons  (I) 


where  p\  =  (xi,j/i)  and  P2  =  3/2)  are  fwo  distinct  points. 


v  2 


Figure  26.  Orientation  of  an  edge 

In  a  polygon  B  we  define  an  exterior  angle ,  <5,-,  as  the  normalized  angle  between 
the  directions  of  an  edge  and  its  previous  one  related  to  vertex  Uj. 

Si  =  $ 

An  exterior  angle  is  positive  or  negative.  It  is  not  equal  to  0  or  i  tt,  because  any 
three  consecutive  vertices  are  not  colinear.  A  vertex  u;,  on  a  polygon  is  said  to  be 
convex  if  all  the  vertices  in  B  are  convex.  Otherwise,  it  is  said  to  be  concave. 
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•  V(v,.  \ ) 

Figure  27.  Interior  and  exterior  angle  of  a  simple  polygon 


Convex  Simple  Polygon 

Figure  28.  Convex  polygon 


2.  Problem  Definition 

Given  v ,  one  vertex  on  a  polygon,  find  a  straight  path  parallel  to  u y1  ( u ) ,  which 
has  a  clearance  of  w0  from  vy>(v)(by  (a,b.a)  representation  configuration).  Track  this 
path  by  using  neutral  switching  method. 

The  approach  we  are  using  for  polygon  tracking  proceeds  through  the  following 
steps-phases:  In  order  for  our  method  to  be  clear  enough  to  the  reader  we  reformulated 
the  previous  phases  in  more  detailed  terms  and  we  decomposed  most  of  the  steps  into 
their  connected  components.  This  way  the  solution  is  developed  in  subsequent  clearer 


sections. 

Phase  1-:  Build  a  polygon  ( B )  world. 
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v2 


Figure  29.  Concave  polygon 

The  world,  illustrated  in  Figure  33,  is  represented  as  a  linked  list  of  polygons, 
where  each  polygon  is  a  doubly  linked  list  of  its  vertices.  Access  to  the  world  is  gained 
through  the  manipulation  of  a  pointer  to  one  of  the  polygons  of  the  list.  (The  reader 
should  be  able  to  find  all  the  necessary  code  for  a  generarion  of  a  polygonal  world  in 
the  Appendix). 


Figure  30.  Representation  of  a  world  data  structure 


Figure  31.  Pointer  manipulation  for  deletion  of  a  node 


Phase  2  :  Compute  one  line(ccw  or  cw)  with  clearance  of  w0,  given  a  vertex  v  in  a 
polygon  J3,  of  a  polygonal  world  W. 

Since  our  motion  planning  problem  emphasizes  ostacle  avoidance  we  compute 
a  safe  path  for  our  vehicle.  In  our  case  the  safest  path  is  one  that  is  on  a  certain 
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Figure  33.  A  safety  path  around  a  polygon 


distance  away  from  the  polygon  we  are  going  to  track.  This  way  the  robot  departs 
from  the  original  path,  and  tracks  the  computed  line  by  simply  specifying  the  safety 
distance. 

Phase  3  :  Compute  5  lines  around  B.  The  various  steps  we  went  through  in  order  to 
compute  the  edges  of  a  polygon  in  our  world  can  be  described  as  follows: 

Tracking  an  edge  vivi+i 

Input:  w0  desired  safety  distance 

Output:  line  the  configuration  of  a  line 

begin 

1.  Define  the  edge  configuration  e; 
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2. 


d  =  (z;i,$(vt-,t>i+i),0) 

Define  the  safety  distace  configuration 
safe  =  (0,  — u>o,0)((— )  for  ccw,  (+)  for  cw) 

3.  line  =  e*  o  safe 

4.  return  line 
end 


Phase  4  :  Track  the  lines  by  using  neutral  switching  method.  For  the  vehicle  to 
exhibit  the  previously  described  problem  of  tracking  a  computed  edge  we  simply 
need  to  specify  the  desired  safety  distance.  Now  its  time  to  show  how  to  specify  the 
desired  path  sequence. 

We  followed  two  methods  as  we  were  proceeding  to  the  solution  of  our  problem: 
(a)using  x*  as  our  calculated  condition,  and  (b)using  steer()  function 
1st  method 

begin 

1.  while  { 

2.  (x0,  y0)  * —  compute  leaving  point 

3.  (x*)  < — compute  x*  switch  line  (i  =  *  +  1) 

4.  apply  next  function  to  advance  the  vehicle  one  step 

5.  } 

end 


Figure  34.  Pseudocode  for  Tracking  a  line  using  the  x *  distance 


2nd  method 

In  the  next  section  of  this  chapter  we  present  simulation  results  of  the  previous 
methods.  To  track  a  polygon  we  used  the  second  previously  described  algorithm(using 
steer ()  function)  for  its  simplicity  and  efficiency. 
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Figure  35.  Tracking  the  edges  of  a  polygon 

begin 

1.  while  i  <  N 

2.  if  steerQ  >=  0 

3.  switch  line  (i  =i  +  1) 

end 

Figure  36.  Pseudocode  for  Tracking  a  line  using  steer()  function 

B.  SUMMARY 

This  chapter  develops  a  method  a  representation  of  a  polugonal  world  suitable 
for  the  motion  of  an  autonomous  robot  lika  Yamabico.  This  world  as  we  saw  is 
a  polygon  with  one  exterior  boundary  polygon,  and  zero  or  more  non-intersecting 
polygons  inside  it.  After  that  we  showed  how  to  track  a  specific  polygon  of  this  world 
in  a  safety  distance  w0  from  it,  using  the  neutral  switching  method. 
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Figure  37.  Tracking  of  four  given  lines 


Figure  38.  Polygon  tracking  by  using  the  neutral  switching  method 
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IV.  HARDWARE  AND  SOFTWARE 
ARCHITECTURE  OF  YAMABICO-11 


In  this  chapter  we  describe  the  hardware  and  software  system  of  the  robot 
Yamabico-11  which  we’re  using  to  test  our  theory  experimentally. 

A.  HARDWARE  SYSTEM 

Yamabico-11  (see  Figure  40)  is  an  automomous,  experimental,  wheeled  robot 
which  has  been  developed  at  the  Naval  Postgraduate  School  (NPS)  over  the  last  years 
under  the  supervision  of  Professor  Yataka  Kanayama. 


Figure  39.  Diagram  of  Yamabico-11  hardware  architecture 

1.  CPU 

Yamabico  is  an  autonomous  robot  and  it  contains  a  Sparc  CPU  which  is  able 
to  run  executable  files  which  have  been  previously  downloaded  via  ethernet.  The 
CPU  is  controlled  through  the  Sparc  Debug  Monitor  via  an  RS-232  connection  to  an 
Apple  Powerbook.  The  Ironies  IV-SPARC-33  is  a  single  processor,  VMEbus  Interface, 
CPU  board.  It  contains  a  25  MHz  SPARC  Integer  Unit,  a  Floating  Point  Unit,  and 
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a  Cache  Controller  and  Memory  Management  Unit.  The  card  installed  in  Yamabico 
has  64  Kbytes  of  cache,  and  16  Mbytes  of  80ns  DRAM.  It  provides  two  RS-232  serial 
I/O  ports,  two  programmable  timers,  and  seven  user-definable  LEDs. 

The  Ironies  SPARC  board  contains  16  Mbytes  of  physical  memory,  yet  pro¬ 
vides  32  bit  addresses  (4  GBytes).  This  4  GBytes  address  space  is  logically  divided 
into  several  regions.  The  three  most  important  regions  are  the  Local  DRAM,  Region 
3,  and  Local  I/O. 

(1) Local  DRAM  The  local  DRAM  is  the  physical  memory  present  on  the  board,  and 
is  addressed  from  0x00000000  to  0x01000000.  The  lower  limit  is  fixed,  while  the  upper 
limit  is  determined  by  the  amount  of  physical  memory  present.  This  space  is  used 
for  the  kernel  and  user  programs,  and  for  allocating  dynamic  memory. 

(2)  Region  3  Region  3  starts  at  the  end  of  region  2,  and  extends  to  the  bottom  of 
the  EPROM  space.  The  default  configuration  provides  addresses  from  OxfcOOOOOO 
to  OxffOOOO,  however,  only  the  upper  boundary  is  fixed.  The  lower  boundary  may 
be  changed  by  writing  to  the  appropriate  register  as  defined  in  the  Ironies  manual. 
MML11  currently  does  not  change  the  default  address  map,  but  does  provide  for  Re¬ 
gion  3  to  be  VME  bus  A16  addressable.  All  devices  on  the  VME  bus  are  addressed 
from  Region  3.  Addresses  are  obtained  by  adding  the  16-bit  base  address  of  a  specific 
hardware  device  to  the  region  3  offset  of  OxfcOOOOOO.  This  includes  the  shaft  encoders, 
quad  serial  boards,  and  sonar  board. 

(3) Local  I/O  The  local  I/O  region  contains  the  addresses  for  the  registers  which  con¬ 
trol  the  operation  of  the  Ironies  SPARC  board.  They  are  addressed  from  OxfffOOOO 
to  Oxffffffff.Both  limits  are  fixed.  Internal  interrupts  are  those  generated  on  the  CPU 
board.  The  two  most  important  are  the  Timer  1  and  Timer  2  interrupts.  Timer  1  can 
be  set  to  provide  interrupts  at  50,  100,  or  1000  hz.  Currently,  MML11  uses  Timer  1  to 
provide  the  10ms  (100  Hz)  motion  control  interrupt.  Timer  2  provides  a  broader  range 
of  interrupts,  and  is  currently  unused.  The  interrupt  vectors  for  Timer  1  and  Timer 
2  are  defined  by  the  Local  Interrupt  Vector  Base  Register(0xfffc0057).  Yamabico  s 
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communication  interface  can  be  accomplished  by  the  help  of  an  input/output  sub¬ 
system  that  provides  three  important  functions;screen  input/output  via  the  on  board 
laptop  computer,  facilities  for  downloading  executable  programs  to  Yamabico  s  main 
memory,  and  functions  for  retrieval  of  sonar  data  collected  by  Yamabico. 

External  interrupts  are  those  generated  off  the  CPU  board.  The  most  impor¬ 
tant  are  from  the  quad  serial  boards,  and  the  sonar  board,  which  are  handled  through 
the  7  VMEbus  Interrupt  Request  lines. 

2.  Wheels 

In  Yamabico-11,  there  are  six  wheels  in  total.  Two  wheels,  lying  on  the  robot’s 
center  line,  drive  the  robot.  The  remaining  four  casters  are  smaller  than  the  drive 
wheels  and  each  has  a  shock  absorbing  material  connected  to  the  robot’s  chassis, 
two  in  the  front  and  two  in  the  rear.  Two  DC  motors  are  used  to  drive  the  wheels, 
one  for  each  drive  wheel.  There  is  also  a  reduction  gear  box  connected  between 
the  motor  and  the  drive  wheel.  The  motor  control  board  controls  the  motors.  In 
Yamabico  the  control  program  is  called  MML  system.  In  this  system  there  is  an 
integer  variable  called  ’pwm.’  The  pwm  value  is  ranged  from  -127  to  127,  its  absolute 
value  represents  the  amount  of  time  the  motor  will  be  activated  and  its  sign  represents 
the  motor  rotational  direction.  The  positive  sign  is  clockwise  for  right  wheel  control 
value(rpwm)  and  is  counterclockwise  for  left  wheel  control  value(lpwm).  The  negative 
sign  is  just  opposite.  Each  motor  is  activated  by  the  amount  of  time  which  the  pwm 
value  represents.  If  we  want  the  motor  to  get  half  of  the  motor’s  maximum  rotational 
speed,  we  will  set  the  pwm  value  to  64  or  -64  and  if  we  want  the  motor  to  get  all  the 
rotational  speed  we  ’ll  have  to  set  it  to  127  or  -127.  The  real  relationship  between 
the  pwm  value  and  the  actual  robot  velocity  are  obtained  by  experiments.  As  we  see 
the  robot’s  control  program  gives  us  the  capability  to  achieve  the  velocity  we  desire 
and  this  capability  is  the  very  basis  of  our  robot  Yamabico-11.  The  Yamabico-11 
hardware  architecture  is  illustrated  in  Figure  39. 
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Figure  40.  Autonomous  mobile  robot,  Yarnabico-11 
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3.  Sonars 

Sensors  include  twelve  sonax  transdusers,  precision  dead-reckoning  wheel  en¬ 
coders,  and  a  color  video  camera.  The  Yamabico’s  software  is  written  in  C,  and 
lately  a  lot  of  efforts  have  been  made  to  upgrade  the  system  so  it’ll  support  C++. The 
motion  of  the  robot  is  achieved  by  an  electric  motor  powered  by  batteries. 


Figure  41.  Yamabico-11  sonax  configuration 


B.  SOFTWARE  ARCHITECTURE  OF  YAMABICO-11 

All  programs  on  the  robot  are  developed  using  a  Sun  3/60  workstation  and 
the  Unix  operating  system.  These  programs  axe  written  in  the  ’C’  programming  lan¬ 
guage,  compiled  and  then  downloaded  to  the  robot  via  a  RS-232  link  at  a  rate  of 
19600  baud.  The  software  system  consists  of  an  operating  system  kernel  and  a  user 
program  loaded  at  different  addresses  in  the  robot’s  main  memory.  The  kernel  needs 
to  be  downloaded  only  once  during  the  course  of  a  given  experiment.  A  user  program 
user(),  can  be  modified  and  downloaded  quickly  to  support  rapid  development.  An 
on  board  notebook  computer  is  provided  to  accomplish  command  level  communica¬ 
tion  to  and  from  the  user.  The  Model  based  Mobile-robot  Language  MML  is  the 
driving  force  behind  the  robot.  It  is  actually  a  reprogrammable  software  system  and 
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a  multitasking  operating  system  that  provides  robot  motion  and  sensor  functions  and 
also  allocates  processing  resources  while  performing  odometry  functions.  Tasks  are 
assigned  according  to  their  priority  level.  This  is  accomplished  with  the  help  of  the 
eight  interrupt  levels  that  the  motorola  CPU  has. 


Figure  42.  MML-11  software  conceptual  architecture 

1.  User  Program  Utility 

We  distinguish  two  kind  of  programs  in  Yamabico’s  software  structure: system 
programs  and  user  programs.  User  programs  control  the  robot  s  motion  in  our  ex¬ 
perimental  environment  by  including  all  the  necessary  instructions.  An  example  of  a 
user()  program  is  given  in  the  APPENDIX. 

2.  Library  Functions 

A  special  importance  should  be  given  to  Yamabico’s  geometric  module  which 
provides  the  necessary  mathematical  support  for  the  various  required  spatial  rea¬ 
soning  tasks.  There  are  three  important  components  in  this  susystem;assignment 
functions  for  specifying  geometric  variables, math  utility  functions  for  manipulating 
the  geometric  variables  and  path  tracking  geometric  support  functions  for  reasoning 
about  path  elements. 
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3.  Functions 

The  path  utility  functions  provide  a  library  of  routines  for  the  algebraic  ma¬ 
nipulation  of  geometric  variables.  For  example, the  composition  function  is  used  to 
perform  2D  transformations  and  the  inverse  function  determines  the  algebraic  inverse 
of  a  given  configuration.  These  functions  support  algebraic  manipulations  for  auto¬ 
matic  dead  reckoning  error  correction.  Also  provided  are  an  assortment  of  utility 
functions  for  spatial  reasoning  math  on  board  Yamabico.  Examples  include  three 
types  of  normalize  functions  and  a  ceiling  function.  All  of  these  utilities  support 
path  tracking  vehicle  control.  The  path  tracking  geometric  support  functions  serve 
to  connect  individual  path  elements  for  smooth  vehicle  motion.  This  subsystem  is 
composed  of  two  types  of  functions  which  are  related. 

4.  Odometry  Capability 

We  should  say  a  few  words  about  how  Yamabico’s  software  system  maintains 
its  odometry  estimate.  A  set  of  functions  are  used  to  provide  automatic  vehicle 
odometry  correction.  What  they’re  doing  actually  is  setting  the  initial  configuration 
of  the  robot,  reading  the  current  value  of  the  robot’s  odometry  estimate  and  updating 
this  value  according  to  the  new  estimate. 
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V.  CONCLUSIONS-FUTURE  RESEARCH 


A.  SUMMARY 

We  conclude  our  thesis  by  summarizing  the  technique  of  neutral  switching 
method  and  its  contributions  to  the  field  of  Robotics.  We  also  give  in  this  chapter  a 
final  reflection  on  the  long-term  aim  of  our  research  in  Intelligent  Robotics. 

The  neutral  switching  method  which  was  thoroughly  presented  in  Chapter  IV 
was  described  by  the  equation  (II.5)  and  is  a  major  contribution  toward  more  au¬ 
tonomous  and  efficient  robots.  We  could  even  say  more  smart  robots.  This  method 
is  extremely  useful  as  we  saw  for  the  motion  control  of  the  autonomous  vehicles  and 
enhances  their  capabilities  by  the  following  way: 

First,  it  gives  a  description  of  the  desired  motion  of  the  vehicle  in  a  more 
transparent  way,  than  any  other  existing  methods. 

With  our  method  the  motion  of  the  robot  is  simply  described  in  a  more  general 
way.  This  gives  the  AI  researcher  the  capability  to  program  and  control  the  behavior 
of  the  vehicle  more  efficiently.  The  movement  of  the  vehicle  along  a  polygonal  world 
is  smoother  and  all  the  target  tracking  maneuveurs  are  being  safer  at  the  desired 
speed. 

Secondly,  another  contribution  of  N.S.M  is  the  safe  path  tracking  where  a  path 
in  our  programs  is  represented  by  a  configuration. 

By  using  the  algorithm  described  in  chapter  VI  we  were  able  to  generate  a 
sequence  of  path  segments  that  represented  the  desired  path  of  the  robot  and,  we  were 
able  to  program  the  robot  on  a  simulator  to  maneuver  on  this  path  and  gradually 
track  a  polygon. 

We  also  saw  how  the  smoothness  a  played  an  important  role  in  our  research 
by  determining  it  as  the  distance  the  robot  moves  along  the  reference  path  before  it 
converges  to  this  path.  After  this  we  examined  what  kind  of  data  structures  were 
suitable- for  representation  of  a  polygonal  world.  This  way  we  gave  the  robot  the 
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capability  to  correctly  perceive  its  surrounding  environment  and  program  its  behavior. 

Autonomous  robots  like  Yamabico,  should  be  enabled  by  their  navigational 
system  to  autonomously  navigated  through  various  worlds  to  their  required  destina¬ 
tions.  This  autonomous  navigation  capability  can  be  used  in  various  applications  such 
as  unmanned  explorations  or  operations  of  dangerous  environments,  manipulation  of 
hazardous  materials  e.t.c 

The  problem  of  robot  navigation  has  been  studied  by  several  researchers  so 
far  as  the  reader  can  very  easily  see  in  the  list  of  references  used  for  this  thesis. 

Robotics  as  a  research  area  of  computer  science  has  been  proven  to  be  full  of 
a  variety  of  issues  ranging  from  abstarct  mathematical  to  highly  realistic  problems. 

The  algorithm  for  polygon  tracking  proposed  by  Professor  Kanayama  based  on 
N.S.M  has  provided  many  practical  advantages  for  the  guiding  of  the  movement  of  an 
autonomous  vehicle.  Fist  of  all  it  is  reliable  and  easy  to  understand  and  implement. 
Secondly,  it  is  very  well  documented  and  it  is  supported  by  a  strong  mathematical 
theory.  Its  contribution  to  the  Robotics  science  is  considered  a  major  one. 

B.  FUTURE  RESEARCH 

As  we  saw  in  chapter  VI  the  key  notion  behind  Neutral  Switcing  Method  is 
selecting  an  appropriate  leaving  point  from  the  current  path  segment  our  vehicle  is 
moving  on  to  the  next  one.  The  method  we  ’re  using  gives  us  a  unique  point  for  each 
path  combination.  The  selection  of  that  point  is  done  when  the  steering  function 
returns  a  “zero  feedback”  value: 

ak 

—  =  — (aA  k  +  bA6  +  cAd)  =  0 
ds 

Another  more  general  and  efficient  approach  would  be  to  consider  the  leaving 
point  calculated  when  the  folowing  condition  is  considered: 

—  —  —  (qAk  +  bA6  +  cAd)  =  min 
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Also  a  future  researcher  should  consider  the  case  where  the  navigation  of  au¬ 
tonomous  mobile  robots  like  Yamabico,  is  not  always  done  in  worlds  whose  models 
are  considered  known  a  priori. 

We  expect  this  Thesis  to  be  useful  as  a  future  reference  by  other  students  who 
are  going  to  have  the  luck  to  work  in  Robotics  field. 
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APPENDIX 


#include  <stdio.h> 

#include  <math.h> 

#define  SO  0.0 

#define  PI  3.1415926536 

tdefine  SQR(x)  x*x 
#define  QUB(x)  x*x*x 
#define  S_Max  500 

FILE  *fl; 

typedef  struct 

{ 

double  x; 
double  y; 

>P0INT; 

typedef  struct 

POINT  p  ; 
double  theta; 
double  kappa; 

Configuration  ; 

/***************************************************************************** 
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Function  name  :  Normalize () 

Purpose  :  This  function  is  used  to  get  the  value  of  sigma  from  the  user 

Parameters  :  - 

Return  Type  :  double 

*****************************************************************************/ 


double  Normalize (double  angle) 

{ 

while  (  angle  >  PI  ) 

•c 

angle  -=  2*PI; 

> 

while  (  angle  <  -PI) 

{ 

angle  +=  2*PI; 

> 

return  angle; 


/***************************************************************************** 
Function  name  :  Get_Circ_Transformation 

Purpose  :  This  function  is  used  to  compute  the  circular  transformation 

Parameters  :  &delta_theta  ,  &delta_s 

Return  Type  :  CONFIGURATION 

** ************** ******************************************************* ******/ 

CONFIGURATION  Get_Circular_Transf ormation(double  del ta_s .double  delta.theta) 

{ 

CONFIGURATION  q; 


q.p.x  =  (1.0  -  ( (delta.theta  *  delta_theta)/6.0)  + 

( (delt a_theta*delta_thet a*delta_theta*delta_theta) /120 . 0) )  *  delt a_s ; 
q.p.y  =  (0.5  -  ((delta_theta  *  delta_theta)/24.0)  + 

((delta_theta*delta_theta*delta_theta*delta_ theta) /720 .0))  *  delta_theta 
*  delta_s; 

q. theta  =  delta_theta; 


return  (q) ; 

> 


52 


/******************************************************************** 

Function  name  :  nextQ 

Purpose  :  apply  steering  function  ,  get  next  configuration 

Parameters  :  &delta_theta,  &delta_s,  double  sigma, double  s 

Return  Type  :  CONFIGURATION 

*********************************************************************/ 

CONFIGURATION  next (double  s, double  sigma  .double  delta_s  .double  delta.theta  , 
CONFIGURATION  ql, CONFIGURATION  q2) 

{ 

double  k.a.b.c.lamda; 


k  =  1.0/sigma; 
a  =  3  *  k; 
b  =  3  *  SQR(k) ; 
c  =  QUB(k); 

lamda  =  **  (a  *  ql. kappa  +  b  *Normalize(ql .theta  -  q2. theta)  + 

c  * (- (ql . p . x-q2 . p . x) *s in (q2 . theta)  +  (ql . p . y-q2 . p . y ) *cos (q2 . thet a) ) ) ; 
ql. kappa  -  ql. kappa  +  lamda  *  delta_s; 
delta.theta  =  ql. kappa  *  delta.s; 

q2  =  Get  _C  ir  cular  JTr  ansf  ormat ion ( delt a_  s , delt  a_ thet  a  ) ; 

ql.p.x  =  ql.p.x  +  q2 . p . x  *  cos(ql .theta)  -  q2.p.y  *  s  in  (ql .theta) ; 
ql.p.y  =  ql.p.y  +  q2.p.x  *  sin (ql .theta)  +  q2.p.y  *  cos (ql .theta) ; 
ql. theta  =  ql. theta  +  q2. theta; 

return  (ql) ; 


/********************************************************************* 
Function  name  :  Get_Input_For_ql() 

Purpose  :  This  function  prompts  the  user  to  enter  the  initial 

configuration  for  ql. 

Parameters  :  - 

Return  Type  :  CONFIGURATION 

********************************************************************** 

CONFIGURATION  Get _ Input _For_ql() 

{ 

CONFIGURATION  ql  ; 
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double  xl ,yl ,thetal ,kl; 


printf  ("\nEnter  the  value  for  xl  please  :  ")  ;  scauf  ("/If 11  ,&xl)  ; 
pr intf ( " \nEnter  the  value  for  yl  please:") ; scauf ("/If" ,&yl) ; 
printf ("\nEnter  the  value  of  the  angle  theta  (deg)for  ql  please 
scanf  ("*/lf "  ,&thetal)  ; 

printf ("\nEnter  the  value  of  the  curvature  for  ql  please 
scanf  ("‘/.If",  &kl); 

ql .p.x  =  xl; 
ql.p.y  =  yl; 

ql. theta  =  thetal*PI/180; 
ql . kappa  =  kl ; 

return  ql; 

> 

/********************************************************************* 
Function  name  :  Get .Input _For_q2() 

Purpose  :  This  function  prompts  the  user  to  enter  the  initial 

configuration  for  q2. 

Parameters  :  - 

Return  Type  :  CONFIGURATION 

********************************************************************/ 

CONFIGURATION  Get .Input _For_q2() 

CONFIGURATION  q2; 
double  x2,y2 ,theta2,k2; 

printf ("\nNow  enter  the  value  for  x2  please  :") ; scanf (  ^lf  ,&x2) ; 
printf  ("\nNow  enter  the  value  for  y2  please  :");  scanf  ("'/.If",  &y2)  ; 
printf ("\nNow  enter  the  value  of  the  angle  for  q2  please  :"); 
scanf  ("’/.If",  &theta2) ; 

printf ("\nEnter  The  value  of  the  curvature  for  q2  please  :"); 
scanf  ("’/.If  ”,&k2); 

q2 . p . x  =  x2; 
q2.p.y  =  y2; 

q2. theta  =  theta2*PI/180; 
q2. kappa  =  k2; 
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return  q2; 


> 


/********************************************************************* 
Function  name  :  Leave 

Purpose  :  This  function  is  used  to  calculate  the  leaving  point 

Parameters  :  CONFIGURATION, CONFIGURATION 

Return  Type  :  double  x, double  y 

********************************************************************/ 


POINT  Leave (CONFIGURATION  ql, CONFIGURATION  q2, double  sigma) 

{ 

POINT  temp; 
double  N,T; 

N=Normalize(q2.theta-ql. theta) ; 

T=sin(N) ; 

temp . x= (cos (q2 . theta) * ( (-ql . p . x*sin (ql . theta)  + (ql . p . y*cos (ql . theta)  ) ) ) - 
cos (ql . theta) * ( (-q2 . p . x*sin(q2 . theta) + (q2 . p . y*cos (q2 . theta) + 
(3*sigma*N) ) ) ) ) /T; 

temp . y= (-sin(ql . theta) * ( (-q2 . p . x*sin (q2 . theta) + (q2 . p . y*cos (q2 .theta) + 
(3*sigma*N)) ) )+ 

sin(q2. theta)* ((-ql.p.x*sin(ql.theta)+(ql.p.y*cos(ql. theta) ))))/T; 

return  temp; 

> 


/********************************************************************* 
Function  name  :  main() 

Purpose  :  This  function  is  the  main  program 

Parameters  :  - 

Return  Type  :  int 

********************************************************************/ 

int  main() 

{ 

double  s; 

double  delta_s ,delta_theta  , sigma; 
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CONFIGURATION  ql,q2; 

fl  =  f open("s20150 .dat'V'w") ; 

ql  =  Get_Input_For_ql() ; 
q2  =  Get_Input_For_q2() ; 

printf  ("\nEnter  the  value  of  sigma  please  :")  ;scanf  ("7,lf"  ,&sigma)  ; 
printf  ( 11  \nEnter  the  value  of  delta_s  please  : ")  ;  scauf  ("'/.If "  ,&delta_s)  ; 

ql.p=  Leave (ql,q2, sigma) ; 

printf  ("\n  x='/,f  y='/,f\n",ql.p.x,ql.p.y) ; 

for(s=0;s<=  S_Max;s+=  delta_s) 

{ 

f printf (fl,"\n'/,15.10f  '/.15 . lOf " ,qi .p.x  ,  ql.p.y); 

ql  =  next(s,sigma,delta_s,delta_theta>ql,q2) ; 

} 

f close(f 1) ; 

printf ("\nOk! Now  let’s  see  the  results . \n") ; 
printf ("\nType  ’gnuplot’  please\n") ; 

> 

#if  0 

Thesis  Research 


:  Tracking_of .Lines 
:  Kar-amanl  i  s  Vasilios 
:  Unix 

:  This  file  contains  all  the  necessary  functions  for  tracking 
four  lines  forming  a  rectangular(intermediate  step  for  polygor 
tracking) using  the  new  method  ’neutal  switching’ 

#endif 


Filename 

Author 

Operating  System 
Description 


#include  <stdio.h> 

#include  <math.h> 

#def ine  PI  3.1415926536 
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#define  SQR(x)  x*x 
#define  QUB(x)  x*x*x 

FILE  *f 1; 

typedef  struct 

{ 

double  x; 
double  y; 

3-POINT ; 

typedef  struct 

POINT  p  ; 
double  theta; 
double  kappa; 
3-CONFIGURATION ; 


Function  name  :  NormalizeO 

Purpose  :  This  function  is  used  to  get  the  value  of  sigma  from  the  user 

Parameters  :  - 

Return  Type  :  double 

*****************************************************************************/ 


double  Normalize (double  angle) 

{ 

while  (  angle  >  PI  ) 

{ 

angle  -=  2*PI ; 

> 

while  (  angle  <  -PI) 
angle  +=  2*PI; 

} 


return  angle; 

> 


/***************************************************************************** 
Function  name  :  Get_Circ_Transf ormation 
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Purpose  :  This  function  is  used  to  compute  the  circular  transformation 

Parameters  :  &delta_theta  »  &delta_s 

Return  Type  :  CONFIGURATION 

^♦^jic*^**********^************************************************************/ 

CONFIGURATION  Get.Circular.Transf ormat ion (double  delta.s, double  delta.theta) 

-c 

CONFIGURATION  q; 


q.p.x  =  (1.0  -  ((delta.theta  *  delta_theta)/6.0)  + 

(  (delta_theta*delta_theta*delta_theta*delta_ theta)  / 120 . 0)  )  *  delta_s ; 
q.p.y  =  (0.5  -  ((delta.theta  *  delta.theta) /24.0)  + 

( (delta_theta*delta_theta*delta_theta*delta_ theta) /720 . 0) )  * 
delta.theta  *  delta.s; 
q. theta  =  delta.theta; 


return  (q) ; 

> 


/******************************************************************** 
Function  name  :  next() 

Purpose  :  apply  steering  function  ,  get  next  configuration 

Parameters  :  double  s, double  sigma  , double  delta.s  , 

double  delta.theta  CONFIGURATION  ql .CONFIGURATION  q2) 
Return  Type  :  CONFIGURATION 

*********************************************************************/ 

CONFIGURATION  next (double  s, double  sigma  .double  delta.s  , 

double  delta.theta  .CONFIGURATION  ql .CONFIGURATION  q2) 

CONFIGURATION  q3; 
double  k.a.b.c.lamda; 

k  =  1.0/sigma; 
a  =  3  *  k; 
b  =  3  *  SQR(k); 
c  =  QUB(k); 

larada  =  -  (a  *  ql. kappa  +  b  *Normalize(ql .theta  -  q2. theta)  + 

c  * (- (ql . p . x-q2 . p . x) *sin (q2 . theta) + (ql . p . y-q2 . p . y) *cos (q2 . theta) ) ) ; 
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ql. kappa  =  ql. kappa  +  lamda  *  delta_s; 
delta.theta  =  ql. kappa  *  delta.s; 

q2  =  Get _Circular_Transformat ion (delta.s, delta.thet a) ; 

ql.p.x  -  ql.p.x  +  q2.p.x  *  cos(ql .theta)  -  q2.p.y  *  sin(ql .theta) ; 
ql.p.y  =  ql.p.y  +  q2.p.x  *  sin(ql .theta)  +  q2.p.y  *  cos (ql .theta) ; 
ql. theta  =  ql. theta  +  q2. theta; 

return  (ql); 

> 


/********************************************************************* 


Function  name 
Purpose 


:  Get .Input _For .Lines () 

:  This  function  prompts  the  user  to  enter  the  initial 
configuration  for  ql. 


Parameters  :  - 

Return  Type  ;  CONFIGURATION 

*********************************************************************/ 


CONFIGURATION  Get. Input .For.Lines () 

{ 

CONFIGURATION  ql  ; 
double  xl,yl,thetal,kl; 

printf  ("\nEnter  the  value  for  x  please  ;");scanf(  1% If  ,&xl)  , 

pr intf ( " \nEnt er  the  value  for  y  please;") ;scanf(  Zlf  >&yl) > 

printf ("\nEnter  the  value  of  the  angle  theta  (deg)for  the  line  please  :"); 

scanf  ("'/.If  ",&thetal)  ; 

printf ("\nEnter  the  value  of  the  curvature  for  the  line  please  :"); 
scanf  ("*/.lf",&kl); 


ql.p.x  =  xl; 
ql.p.y  =  yl; 

ql. theta  =  thetal*PI/180; 
ql. kappa  =  kl; 

return  ql; 

> 


/********************************************************************* 
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Function  name 
Purpose 


:  Leave 

:  This  function  is  used  to  calculate  the 
leaving  point 
Parameters  :  CONFIGURATION, CONFIGURATION 

Return  Type  :  double  x, double  y 

* ************* *********** ************************************ *******/ 

POINT  Leave (CONFIGURATION  ql .CONFIGURATION  q2, double  sigma) 

{ 

POINT  temp; 
double  N,T; 

N=Normalize(q2 .theta-ql .theta) ; 

T=sin(N) ; 

temp.x=(cos(q2 . theta)* ((-ql .p.x*sin(ql .theta)+(ql ,p.y*cos(ql .theta) )))- 
cos (ql . theta) * ( (-q2 . p . x*sin(q2 . theta) + (q2 . p . y*cos (q2 . theta) + 
(3*sigma*N) ) ) )) /T; 

t emp. y= (-sin (ql. theta)  *((-q2.p.x*sin(q2. theta)  +  (q2.p.y*cos(q2. theta)  + 
(3*sigma*N) ) ) )+ 

'sin(q2 . theta) * ( (-ql . p . x*sin(ql . theta) + (ql . p . y*cos (ql . theta) ) ) ) ) /T; 
return  temp; 

> 


/********************************************************************* 
Function  :  main() 

Purpose  :  This  function  is  the  main  program 

Parameters  :  - 

Return  Type  :  int 

********************************************************************/ 
int  main() 

-c 

double  s,  xx ; 

double  delta.s, delta. theta, sigma; 
int  i  =  0; 
const  int  N=4; 

CONFIGURATION  qo , qv , ql ; 

CONFIGURATION  q[N+2] ; 
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fl  =  fopen("p.dat","w"); 
f or (i=0 ; i<N ; i++) 

q[i]=Get .Input _For_Lines() ; 

> 

/*  necessary  for  the  tracking  to  close  nicely  by  corresponding 
q [0]  =q [4]  and  q[l]=q[5]  */ 
q[N]=q[0]  ; 
q[N+l]=q[l]  ; 

/*  qv  is  the  actual  point  we;re  on  */ 
qv=q [0] ; 

/*  printing  the  starting  point  */ 

printf ("\nThe  coordinates  of  the  starting  point  are  :\n"); 
printf  ("\nqv:  x=‘/.f  y=*/.f  \n" ,qv.p.x,qv.p.y) ; 

printf  ("\nEnter  the  value  of  sigma  please  :  ")  ;  scanf  ("/If 11  ,&sigma)  , 
printf ("\nEnter  the  value  of  delta.s  please  :")>scanf(  /If  ,&delta_s) > 

/*  calculating  the  first  leaving  point  */ 
qo.p=  Leave (q [0] ,q[l] , sigma) ; 
qo. theta  =  q[0]  .theta; 
qo. kappa  =  q[0] .kappa; 

/*  printing  the  coordinates  of  the  first  leaving  point  */ 
printf ("\nThe  coordinates  of  the  first  leaving  point  qo  are  :\n"); 
printf  ("\n  x=*/.f  y=*/.f  ",qo.p.x,qo.p.y) ; 

i  =  0; 

while(i<=N+l) 

{ 

f printf  (f  1 , "\n'/,15 .  lOf  7.15  .  lOf  "  .qv.p.x  ,  qv.p.y); 

xx  =  (qo .p .x-qv.p .x)*cos (qv . theta)+(qo .p .y-qv.p .y)*sin(qv. theta) ; 
if(xx  <  0.0) 

i++;  /*  switching  lines  */ 
qo  =  q[i]  ; 

qo.p=  Leave(q[i] ,q[i+l] , sigma) ; 
qo. theta  =  q[i] .theta; 
qo. kappa  =  q[i] .kappa; 
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if  (i<4) 

printf ("\nThe  coordinates  for  the  line  i=’/,d  x=*/,f  y=y,f\n", 
i+l,qo.p.x,qo.p.y); 

} 

qv  =  next  ( s , s igma ,  del t  a_  s ,  delt a_ t het  a ,  qv ,  qo  )  ; 


fclose(fl) ; 

printf ("\nOk! Now  let's  see  the  result s. \n" ) ; 
printf ("\nType  'gnuplot'  please\n") ; 


#if  0 


Thesis  Research 


Build_of .Polygon . c 
Karamanlis  Vasilios 
Unix 

This  file  contains  all  the  necassary  functions  to  build  a 
polygon,  using  old  data  structures (not  Lombardo's) 

#endif 


#include  <stdio.h> 
#include  <math.h> 
#include  <stdlib.h> 

FILE  *f4; 

typedef  struct  { 
double  x; 
double  y; 

> 

POINT; 

typedef  struct  { 
POINT  point; 
double  theta; 
double  kappa; 


Filename  : 
Author  : 
Operating  System  : 
Description  : 
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> 

CONFIGURATION; 
typedef  struct  Vertex{ 


POINT 

coord; 

int 

name; 

int 

convex ; 

struct 

Vertex 

♦Next ; 

struct 

Vertex 

♦Prev; 

> 

Vertex; 

typedef  struct  Polygon{ 
int 
int 

struct  Vertex 
struct  Polygon 
> 

Polygon ; 

typedef  struct  { 

int  name ; 

Polygon  *PolygonList ; 

> 

World; 

double  WorldDataC]  = 

{1.0,  0.0,  4.0, 

153.2,  118.0, 

397.2,  118.0, 

397.2,  182.0, 

153.2,  182.0 

>; 


name; 

mode; 

♦VertexList ; 
♦Next ; 


/********************♦♦♦♦♦♦♦♦***♦*****♦***************** 
Function  name  :  OpenFile 

Purpose  :  This  function  is  used  to  open  a  file 

Parameters  :  - 

Return  Type  :  - 

*****************♦♦♦♦♦♦**♦*****************************/ 
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void  OpenFileO 

-c 

f4=  fopen("p.dat" ,  "w")  ; 

> 


/************************************************************** 
Function  name  :  buildWorld 

Purpose  :  This  function  is  used  to  build  a  polygon 

Parameters  :  - 

Return  Type  :  World 

* * * * * **** * ** * * * ** * ** *** * * ****** * ***** * ********** * * * ***** * ***** / 
World* 

buildWorld (World  *curWorld) 

{ 

int 
int 

double 

Polygon 
Vertex 
Vertex 
Vertex 
Vertex 
int 
int 


Vert excount ; 
polycount ; 

tempX,  tempY  ,tempxo  ,tempyo; 

♦curPoly; 

♦curVertex; 

♦PrevVertex; 

♦NextVertex ; 

♦First Vert ex; 
i,  numvert,  numpoly; 
nvert,  nconvexvert; 


Vertexcount  -  0; 
polycount  =  0; 
nconvexvert  =  0; 
i  =  0; 

curWorld  =  (World*)malloc(sizeof (World)) ; 
curPoly  =  curWorld->PolygonList  = 
(Polygon  *)  malloc(sizeof (Polygon)) ; 
numpoly  =  (int)  WorldData[i++] ; 

/*  loop  until  all  Polygons  are  read  */ 
while  (1)  { 

curPoly->name  =  (int)  WorldData[i++] ; 
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numvert  =  nvert  =  (int)  WorldData[i++] ; 
polycount++ ; 

curVertex  =  curPoly->VertexList  = 
(Vertex  *)  malloc(sizeof (Vertex)) ; 


tempxo=WorldData[3] ; 
tempyo=WorldData[4]  ; 


/*  loop  until  all  vertices  for  this 
Polygon  are  read  */ 

while  (numvert  >  0)  { 
tempX  =  WorldData[i++] ; 
tempY  =  WorldData[i++] ; 

Vertexcount++ ; 
curVertex->name  =  Vert excount; 
curVertex->coord.x  =  tempX; 
curVertex->coord.y  =  tempY; 
if (curPoly->name  ==0) 

fprintf (f4,"\n  */,f  */.f " ,curVertex->coord.x, 
curVertex->coord.y) ; 


numvert — ; 


/*  last  Vertex  read  for  this  Polygon  */ 
if (numvert  ==  0) 

{ 

curVertex->Next  =  curPoly->VertexList ; 
curPoly->VertexList->Prev  =  curVertex; 

PrevVertex  =  curVertex; 

curVertex  =  FirstVertex  =  curPoly->VertexList ; 

NextVertex  =  curVertex->Next ; 

fprintf  (f 4 , " \n  */.f  */.f  " , t empxo , tempyo)  ; 

} 

else 

curVertex->Next  =  (Vertex  *)  malloc(sizeof (Vertex)) ; 
PrevVertex  =  curVertex; 
curVertex  =  curVertex->Next ; 
curVertex->Prev  =  PrevVertex; 

} 

> 

numpoly — ; 
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if  (numpoly  <=  0)  { 

curPoly->Next  -  NULL; 
return  curWorld; 

> 

else  { 

curPoly->Next  =  (Polygon  *)  malloc(sizeof (Polygon) ) ; 
curPoly  =  curPoly->Next ; 

Vertexcount  =  0; 
nconvexvert  =  0; 

> 

} 

return  curWorld; 

> 

/************************************************************** 
Function  name  :  main 

Purpose  :  This  function  is  the  main  file 

Parameters  :  - 

Return  Type  :  - 

***************************************************************/ 

int  main (void) 

{ 

World  *curWorld; 

OpenFileO  ; 

curWorld=buildWorld(curWorld) ; 
f close (f 4) ; 

> 


#if  0 


Filename 

Author 

Operating  System 
Description 


Thesis  Research 


:  Polygon. c 
:  Karamanlis  Vasilios 
:  Unix 

:  This  file  contains  all  the  necessary  functions  to  build 
a  polygon  (using  Lombardo's  data  structures) and  create 


66 


path  segments  at  a  given  safety  distance  from  it. After 
that  it  uses  the  neutral  switching  method  to  track  it . 
(It  uses  :"Leave"  function  and  x*  distance) 


#endif 

#include  <stdio.h> 

#include  <math.h> 

/*  manifest  constants  */ 

#def ine  FAILURE  -1 

#def ine  PI  3.1415926536 
tdefine  SQR(x)  x*x 
#define  QUB(x)  x*x*x 
FILE  *f  1 ; 

/♦  typedefs  */ 
typedef  struct  point  { 
double  x,y; 

}  Point; 

typedef  struct  vertex  { 

Point  point; 
struct  vertex 
♦previous, 

♦next ; 

}  Vertex; 

typedef  struct  { 

Point  point; 
double  theta; 
double  kappa; 

>C0NFIGURATI0N  ; 

typedef  struct  polygon  { 
int  degree; 

Vertex  *vertex_list ;  /*  this  is  the  first  vertex  */ 

struct  polygon 
♦previous, 

♦next ; 

}  Polygon; 
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typedef  struct  world  { 
int  degree; 

Polygon  *poly_list; 

>  World; 


/******************************************************* 
Function  name  :  OpenFile 

Purpose  :  This  function  is  used  to  open  a  file 

Parameters  :  - 

Return  Type  :  - 

*******************************************************/ 
void  OpenFileO 

fl=  f open("pl .dat" , "w")  ; 

> 


/♦St!************************************************************************** 


Function  name 

Parameters 

Purpose 

Called  by 


fatal  () 
char  ^message 

on  fatal  error  print  message  then  print  system  message 
then  exit 

create_world()  <world.c> 
create_polygon()  <world.c> 

add_vertex_to_polygon()  <world.c> 


Calls  :  NONE 

************SMC***************************************************************/ 


void  fatal (message) 
char  *message; 

{ 

fprintf (stderr,  "Fatal  error  occured:  "); 
perror (message) ; 
exit (FAILURE) ; 

> 


/***************************************************************************** 
Function  :  create_world() 
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Purpose  :  create  instance  of  a  world 

Returns  :  World  * 

Called  by  :  ANYBODY 

Calls  :  fatal ()  <utilities . c> 

Comments  :  this  function  allocates  space  for  a  world  and  returns  a  pointer 

I******************************************************************************/ 

World  *create_world() 

{ 

World  *w; 

/*  allocate  memory  for  a  world  */ 

if((w  =  (World  *)malloc(sizeof (World)))  ==  NULL)  { 

fatal ("create_world:  malloc\n") ; 
exit (FAILURE) ; 

> 

/*  initialize  fields  */ 
w->degree  =  0; 
w->poly_list  =  NULL; 

return (w) ; 

> 

Function  name  :  Composition 

Purpose  :  This  function  is  used  to  compute  the  composition 

of  two  given  transformations 

Parameters  :  two  two  dimensional  coordinate  transf ormations 

Return  Type  :  CONFIGURATION-the  composition  of  the  two 

transformations  ql  and  q2 

***************************************************** ********** *********/ 

CONFIGURATION  compos it i on (ql ,q2) 

CONFIGURATION  ql; 

CONFIGURATION  q2; 

{ 

CONFIGURATION  q3; 

q3. point. x  =  ql. point. x  +  q2. point. x  *  cos (ql .theta)  -  q2. point. y 
*  s in (ql .theta) ; 
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q3. point. y  =  ql. point. y  +  q2. point. x  *  sin(ql .theta)  +  q2. point. y 

*  cos (ql .theta) ; 
q3. theta  =  ql. theta  +  q2. theta; 

return  q3; 

> 

/************************************************************************* 
Function  name  :  Get.Circ.Transformation 

Purpose  :  This  function  is  used  to  compute  the  circular 

transformation 

Parameters  :  ftdelta. theta  ,  &delta_s 

Return  Type  :  CONFIGURATION 

***************************************************************************/ 

CONFIGURATION  Get_Circular_Transformation(delta_s .delta.theta) 
double  delt a. s .delta.theta; 

{ 

CONFIGURATION  q; 

q. point. x  =  (1.0  -  ((delta.theta  *  delta_theta)/6 . 0)  + 

( (delta. theta*delta_theta*delta_theta*delta_theta)/120.0)) 

*  delta.s; 

q. point. y  =  (0.5  -  ((delta. theta  *  delta_theta)/24 .0)  + 

( (delt  a.thet  a*delt  a.thet a*delt a.thet a*delt  a.thet a) /720 . 0) ) 

*  delta.theta  *  delta.s; 
q. theta  =  delta.theta; 

return  (q) ; 

} 

/***************************************************************************** 
Normalize () 

This  function  is  used  to  get  the  value  of  sigma  from  the  user 
_ _rfjr  double 

********V********************************************************************/ 

double  Normalize (angle) 
double  angle; 


r unction  name 
Purpose 
Parameters 
Return  Tvne 
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while  (  angle  >  PI  ) 

{ 

angle  -=  2*PI ; 

> 

while  (  angle  <  -PI) 

{ 

angle  +=  2*PI; 

> 

return  angle; 

> 


/******************************************************************** 


Function  name  :  next() 

Purpose  :  apply  steering  function  ,  get  next  configuration 

Parameters  :  double  s, double  sigma  , double  delta_s  , 

double  delta.theta  , CONFIGURATION  ql, CONFIGURATION  q2) 


Return  Type  :  CONFIGURATION 

*********************************************************************/ 


CONFIGURATION  next (sigma  ,delta_s  ,delta_theta  ,ql,q2) 
double  sigma  ,delta_s  ,delta_theta  ; 

CONFIGURATION  ql,q2; 

CONFIGURATION  q3; 
double  k,a,b,c3lamda; 


k  =  1.0/sigma; 
a  =  3  *  k; 
b  =  3  *  SQR(k); 
c  =  QUB(k) ; 

lamda  =  -  (a  *  ql. kappa  +  b  *Normalize(ql .theta  -  q2. theta)  + 
c  * (- (ql .point . x-q2 . point . x) *sin(q2 .theta) + 

(ql . point . y-q2 . point . y ) *cos (q2 . theta) ) ) ; 

ql. kappa  =  ql. kappa  +  lamda  *  delta.s; 
delta.theta  =  ql. kappa  *  delta.s; 

q2  =  Get_Circular_Transf ormat ion(delta_s ,delta_theta) ; 

ql. point. x  =  ql. point. x  +  q2. point. x  *  cos(ql .theta)  -  q2. point. y 
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*  sin (ql .theta) ; 

ql. point. y  =  ql. point. y  +  q2. point. x  *  sin(ql .theta)  +  q2. point. y 

*  cos (ql .theta) ; 
ql. theta  =  ql. theta  +  q2. theta; 

return  (ql) ; 

> 

/********************************************************************* 
Function  name  :  Leave 

Purpose  :  This  function  is  used  to  calculate  the 

leaving  point 

Parameters  :  CONFIGURATION, CONFIGURATION 

Return  Type  :  double  x, double  y 

********************************************************************/ 

Point  Leave (ql,q2, sigma) 

CONFIGURATION  ql,q2; 
double  sigma; 

{ 

Point  temp; 
double  N,T; 

N=Normalize(q2 .theta-ql. theta) ; 

T=sin(N) ; 

temp . x= (cos (q2 . theta) * ( (-ql . point . x*sin (ql . theta)  + 

(ql .point .y*cos(ql .theta))))- 

cos (ql . theta) * ( (-q2 . point . x*sin (q2 . theta) + 

(q2 .point .y*cos(q2 .theta)+(3*sigma*N) ))))/T ; 
temp .  y= (-sin(ql . theta) * ( (-q2 . point . x*sin (q2 . theta)  + 

(q2 . po int . y*  cos (q2 . thet  a)  +  (3*  s igma*N ) ) ) )  + 

sin(q2. theta)*((-ql. point .x*sin(ql.theta)+ 

(ql . po int . y*cos (ql . theta) ) ) ) ) /T ; 

return  temp; 

> 


/**************************************************************************** 
Function  :  create_polygon() 
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Purpose  :  create  instance  of  a  polygon 

Returns  :  Polygon  * 

Called  by  :  ANYBODY 

Calls  :  fatal ()  < utilities . c> 

Comments  :  this  function  allocates  space  for  a  polygon  and 

returns  a  pointer  to  it 

******* ************************************************************** ********/ 

Polygon  *create_polygon() 

{ 

Polygon  *p; 

/*  allocate  memory  for  a  polygon  */ 

if((p  =  (Polygon  *)malloc(sizeof (Polygon)))  ==  NULL)  { 

fatal ("create.polygon:  malloc\n") ; 
exit (FAILURE) ; 

> 

/*  initialize  fields  */ 
p->degree  =  0; 
p->vertex_list  =  NULL; 
p->previous  =  NULL; 
p->next  =  NULL; 

return (p) ; 

> 

Function  :  add_vertex_to_polygon(x,  y,  p) 

Parameters  :  double  x  x  coordinate  of  new  vertex 

double  y  y  coordinate  of  new  vertex 
Polygon  *p  pointer  to  an  existing  polygon 
Purpose  :  add  a  vertex  to  sin  existing  polygon 

Returns  :  void 

Called  by  :  ANYBODY 

Calls  :  fatal ()  <utilities . c> 

Comments  :  adds  vertex  to  the  end  of  the  vertex  list  -  no  way  to  insert 

NOTE:  polygon  must  exist  before  adding  vertices 
*****************************************************************************/ 
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void  add_vertex_to_polygon(x,  y,  p) 
double  x,  y; 

Polygon  *p; 

{ 

int  i;  /*  loop  variable  */ 

Vertex 

♦new.vertex,  /♦  pointer  to  the  new  vertex  */ 

♦current .vertex;  /*  pointer  to  the  current  vertex  */ 

/♦  allocate  space  for  the  new  vertex  */ 

if  ( (new.vertex  =  (Vertex  *)malloc(sizeof (Vertex)))  ==  NULL)  { 
fatal("add_vertex_to_polygon:  malloc\n") ; 
exit (FAILURE) ; 

> 

/♦  install  coordinates  */ 
new_vertex->point .x  =  x; 
new_vertex->point .y  =  y; 

/♦  check  if  first  vertex  ♦/ 
if(p->degree  ==  0)  { 

new. vert ex->previous  =  new.vertex; 
p->vertex_list  =  new.vertex; 

> 

else  { 

/♦  set  up  the  links  */ 

current .vert ex  =  p->vertex_list; 

current .vertex- >previous  =  new.vertex; 

/♦  find  the  last  vertex  ♦/ 
for(i  =  1;  i  <  p->degree;  i++) 

current .vertex  =  current _vertex->next; 

new_vertex->previous  =  current .vert ex; 
current .vert ex- >next  =  new.vertex; 


> 

new. vert ex->next  =  p->vertex_list; 
p->degree++; 

> 
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/ **************************************************************************** 
Function  :  display_vertices_of_polygon(p) 

Parameters:  Polygon  *p  pointer  to  an  existing  polygon 

Purpose  :  display  the  vertices  of  a  polygon  of  the  existing  world 

Returns  :  void 

Called  by  :  ANYBODY 

Calls  :  NONE 

Comments  : 

********************************************  *********^******* 

void  display_vertices_of_polygon(p) 

Polygon  *p; 

{ 

Vertex  ^current,  *first; 

f irst=current=  p->vertex_list ; 
do{ 

printf  ("\n%f  */,f\n",current->point.x  ,  current->point .y) ; 
current  =  current - >next ; 

}while( current  !=  first); 
return; 

> 

/ **************************************************************************** 
Function  :  add_polygon_to_world(p,  w) 

Parameters:  Polygon  *p  pointer  to  an  existing  polygon 

World  *w  pointer  to  an  existing  world 
Purpose  :  add  an  existing  polygon  to  an  existing  world 

Returns  :  void 

Called  by  :  ANYBODY 

Calls  :  NONE 

Comments  :  adds  polygon  to  end  of  polygon  list  -  no  way  to  insert  polygon 

*****************************************************************************/ 

void  add_polygon_to_world(p,  w) 

Polygon  *p; 

World  *w; 
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Polygon  ^current .polygon;  /*  pointer  to  current  polygon  */ 


/*  check  if  first  polygon  */ 
if(w->degree  ==  0)  { 
w->poly_list  =  p; 

} 

else  { 

/*  find  the  last  polygon  */ 
current .polygon  =  w->poly_list ; 
while (current _polygon->next  !=  NULL) 

current .polygon  =  current _polygon->next; 

p->previous  =  cur rent .polygon; 
current _polygon->next  =  p; 

} 

w->degree++; 

> 

Function  :  Find.Path.Segment (v) 

Parameters:  vertex 

Purpose  :  computes  the  path  segment  according  to  a  given  safety  distance 

Returns  :  CONFIGURATION 

Called  by  :  ANYBODY 

Calls  :  NONE 

Comments  :  safety  distance  is  -50  by  default (-  shows  ccw  polygon, +  cw  ) 

CONFIGURATION  Find.Path.Segment (v) 

Vertex  *v; 

{ 

CONFIGURATION  safe; 

CONFIGURATION  edge; 

safe . point . x=0 . 0 ; 
safe . point . y=-50 . 0 ; 
safe.theta=0.0; 
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safe.kappa*0 .0; 

edge . point . x=v->point . x ; 

edge . point . y=v->point . y ; 

edge. theta*  atan2(v->next->point .y  -  v  ->point.y, 

v->next->point .x  -  v->point.x); 


edge . kappa* 0 . 0 ; 


return(composition(edge,safe)) ; 

> 


/******************************************* 
Function  name  :  main() 

Purpose  :  this  is  the  main  function 

*******************************************/ 


int  main() 

{ 

Vertex  *new_ vertex; 

Polygon  *pl; 

World  *w; 

Vertex  *v,*first; 
double  X,Y,s,xx; 

double  delta_s,delta_theta, sigma; 
int  i=0; 
int  N=4; 

CONFIGURATION  e,q[6] ,qo,qv; 

/*  make  a  world  */ 
w  =  create_world() ; 


/*  make  a  polygon  */ 
pi  =  create_polygon() ; 


/*  add  three  vertices  */ 
add_vertex_to_polygon(153 . 2 , 
add_vertex_to_polygon(397 .2, 
add_vertex_to_polygon(397 .2, 
add_vertex_to_polygon(153 . 2 , 


118.0,  pi); 
118.0,  pi); 
182.0,  pi) ; 
182.0,  pi); 


display_vertices_of _polygon(pl) ; 
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/*  attach  polygon  to  world  */ 
add_polygon_to_world(pl,  w) ; 

OpenFileO ; 

v=first  =  pl->vertex_list ; 
do{ 

q[i]  =  Find_Path_Segment(v) ; 

X  =  q[i] .point .x  ; 

Y  =  q[i] . point. y  ; 
printf("7.f  y.f\n",X,Y); 
v  =  v->next ; 
i++; 

}while(  v  !=  first); 
q[N]=q[0]  ; 
q[N+l]=q[l]  ; 

/*  qv  is  the  actual  point  we’re  on  */ 
qv=q[0]  ; 

printf  ("\nEnter  the  value  of  sigma  please  :");  scanf  ("*/.lf"  ,&sigma)  ; 
printf  ("\nEnter  the  value  of  delta.s  please  scanf  ("’/.If",  ftdelta.s)  ; 

/*  calculating  the  first  leaving  point  */ 
qo.point=  Leave (q[0] ,q[l] , sigma) ; 
qo. theta  =  q[0] .theta; 
qo. kappa  =  q[0] .kappa; 

i=0; 

while(i<=N+l) 

{ 

f printf  (fl ,  "\n'/,15 .  lOf  */.15 . 10f"  ,qv. point  .x  , qv. point  .y)  ; 

/♦printf  ("\n'/,15 . lOf  */,15 .  lOf" ,qv. point  .x  ,  qv. point  .y)  ;*/ 
xx  =  (qo. point .x-qv. point .x)*cos(qv. theta)+(qo. point .y-qv. point .y) 
*sin(qv. theta) ; 
if(xx  <  0.0) 

i++;  /*  switching  lines  */ 
qo  =  q[i]  ; 

qo.point=  Leave (q[i] ,q[i+l] , sigma); 
qo. theta  =  q[i] .theta; 
qo. kappa  =  q[i] .kappa; 

-  > 
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qv  =  next(sigma,delta_s,delta_theta,qv,qo) ; 


> 

fclose(f 1) ; 
return  0; 


#if  0 


Thesis  Research 


Filename 
Author 
Project  #3 
Operating  System 
Description 


#endif 


:  main4 . c 

:  Karamanlis  Vasilios 
:  Circle  Tracking 
:  Unix 

:  This  file  contains  all  the  necessary  functions  for 
circle  tracking 

(Using  the  Neutral  Switching  method) 


#include  <stdio.h> 
#include  <math.h> 
#include  <stdlib.h> 
#include  <time.h> 


#define  SO  0.0 
#define  S_MAX  600.0 
#def ine  PI  3.1415926536 
#define  RAD  180.0/PI 


FILE  *fl  ; 

/*  Data  structures  for  defining  coordinates  X  ,  Y  and  angle  theta  */ 

typedef  struct 

double  X; 
double  Y ; 

}P0INT; 

typedef  struct 
{  - 
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POINT  point; 
double  theta; 
double  kappa; 
3-CONFIGURATION; 


/*************************************************************************** 
Function  name  :  Get_Circ_Transf ormation 

Purpose  :  This  function  is  used  to  compute  the  circular  transformation 

Parameters  :  &delta_theta  ,  &delta_s 

Return  Type  :  CONFIGURATION 

***************************************************************************/ 


CONFIGURATION  Get_Circular_Transf ormation(double  ds, double  dt) 
CONFIGURATION  q; 

q. point. X  =  (1.0  -  (dt  *  dt)/6.0)  *ds; 
q. point. Y  =  (0.5  -  (dt  *  dt)/24.0)*dt*ds; 
q. theta  =  dt ; 


return  (q) ; 

> 


/*************************************************************************** 
Function  name  :  Print_to_File 

Purpose  :  This  function  is  used  to  write  the  data  to  the  output  file 

Parameters  :  f,q 

Return  Type  :  - 

***************************************************************************/ 

void  Print_to_File(FILE  *f, CONFIGURATION  q) 

{ 

fprintf (f ,"\n’/.f  If"  ,  q. point. X  ,  q. point. Y); 

> 

/*************************************************************************** 
Function  name  :  NormalizeO 

Purpose  :  This  function  is  used  to  get  the  value  of  sigma  from  the  user 

Parameters  :  - 

Return  Type  :  double 
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*********************** 


****************************************************/ 


double  Normalize (double  angle) 

{ 

while  (  angle  >  PI  ) 

■C 

angle  -=  2*PI ; 

> 

while  (  angle  <  -PI) 

■C 

angle  +=  2*PI; 

> 

return  angle; 


/*************************************************************************** 
sgn() 


Function  name 
Purpose 
Parameters 
Return  Type  :  - 

***************************************************************************/ 


This  function  is  used  to  return  the  sign  of  a  value 
double 


int  sgn (double  v) 

{ 

int  SIGN; 

if (v==0.0)SIGN  =0.0; 

else  if (v>0 . 0)SIGN  =  1; 
else  SIGN  =  -1; 

return  (SIGN); 

> 


/*************************************************************************** 
Function  name  :  Circle_Tracking() 

Purpose  :  This  function  is  used  to  get  the  next  configuration 

Parameters  :  double  s, double  sig, double  ds, double  dt .CONFIGURATION  ql,q2 
Return  Type  :  CONFIGURATION 

***************************************************************************/ 


CONFIGURATION  Circle_Tracking(double  s, double  sigma, double  ds, double  dt. 
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CONFIGURATION  ql .CONFIGURATION  q2) 


CONFIGURATION  qc; 

double  k,kl,a,b,c,A,E,D,Dl,rad,Nl,N; 

int  SGN; 

sigma=0 .5; 

rad=10.0; 

k=l .O/sigma; 

kl=l/rad; 

a-3*k; 

b=3* (k*k) - (kl*kl ) ; 
c=(k*k*k)-3*k*(kl*kl) ; 

SGN  =  sgn(rad) ; 
f or (s=SO ; s<=200 ; s+=ds) 

{ 

Print_to_File(f 1 ,ql) ; 

E=atan2((q2 .point .Y-ql .point .Y) , (q2 .point .X-ql .point .X)) ; 

Dl= Cq2. point .X-ql .point .X)*(q2 .point .X-ql. point .X)+(q2 .point .Y 
-ql .point . Y)*(q2. point .Y-ql .point .Y) ; 

D=sqrt (Dl) ; 

Nl=ql .theta-(E-(SGN*PI/2)) ; 

N=Normalize(Nl) ; 

A=-(a*(ql .kappa-1 . 0/rad) +b*N+c*(rad-SGN*D)) ; 

ql . kappa=ql .kappa+A*ds ; 

dt=ql.kappa*ds; 

qc=Get _Circular_Transf ormat ion (ds , dt ) ; 

ql. point. X  =  ql. point. X  +qc. point .X*cos(ql. theta)  -  qc. point. Y 
*sin(ql .theta) ; 

ql. point. Y  =  ql. point. Y  +qc. point .X*sin(ql. theta)  +  qc. point. Y 
*cos(ql .theta) ; 

ql. theta  =  ql. theta  +qc. theta; 

} 

return  (ql) ; 

> 


/*************************************************************** 
Function  name  :  main() 

Purpose  :  main  program 

Parameters  :  - 
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Return  Type  :  int 

sic**************************************************************/ 

int  mainO 

{ 

/*  declarations  of  the  variables  */ 
double  s; 

double  delta_s ,delta_theta, sigma; 

CONFIGURATION  ql , q2 , qpoint ; 

/*  declares  the  values  used  in  the  program  */ 
delta_s=0.5;  /*  value  for  the  length  */ 
ql  .point .Y=0.0;  /*  Y  value  of  the  vehicle  */ 

/*ql .point .X=-14.53;*/  /*  X  value  of  the  vehicle  for  sigma=l .0*/ 

/*ql .point .X=-16.8;*/  /*  X  value  of  the  vehicle  for  sigma=1.5*/ 

/*qi .point .X=-19.2;*//*  X  value  of  the  vehicle  for  sigma=2.0*/ 
ql .point .X=-12.3;/*  X  value  of  the  vehicle  for  sigma=0.5*/ 
ql .theta=0.0;  /*  theta  value  of  the  vehicle  */ 
ql . kappa=0 . 0 ;  /*  kappa  value  of  the  vehicle  */ 
qpoint .point. X=.0;  /*  X  value  of  the  center  of  the  circle  */ 
qpoint. point. Y=0.0;  /*  Y  value  of  the  center  of  the  circle  */ 
qpo int . thet a=0 . 0 ;  /*  theta  value  of  the  center  of  the  circle  */ 

qpoint . kappa=0 . 0 ;  /*  kappa  value  of  the  center  of  the  circle  */ 

/*  prints  the  current  date  and  time  */ 

time_t  now; 

now  =  time (NULL) ; 

printfC'The  current  date  and  time  is  :  ‘/.s"  ,ctime(&now))  ; 

/*  welcomes  the  user  to  the  program  */ 

printf ("\nWelcome  to  my  Circle  tracking  program  in  C.\n"); 

printf ("\nYou  are  going  to  deal  with  advanced  Robotics  material. \n  ), 

printf ("\nl  think  you  are  a  little  bit  nervous. \n") ; 

printf ("Xnlt's  not  a  big  deal, just  only  ADVANCED  R0B0TICS\n") ; 

printf ("\nLet ’ s  start !\n"); 

/*  opens  the  output  file  */ 
fl=f  open  ("path.  dat'V'w")  ; 

/*  makes  the  circle  tracking  */ 

q2=Circle_Tracking(s .sigma , delta. s , delt a.theta , ql , qpoint) ; 

Print _to_File(f 1 ,q2) ; 

/*  closes  the  output  file*/ 
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f close(f 1) ; 


/*  prompts  the  user  to  use  'gnuplot'  drawing  program*/ 
print f ( "\nOk ! Now  let's  see  the  results\n"); 
printf ("\nType  'gnuplot'  please. \n") ; 


#if  0 


Thesis  Research 


Filename 

Author 

Operating  System 
Description 


Polygon. c 

Karamanlis  Vasilios 
Unix 

This  file  contains  all  the  necessary  functions  to 
build  a  polygon  and  create  path  segments  at  a  given 
safety  distance  from  it. After  that  it  uses 
* ‘ Neutral  switcing  method''  to  track  it. 


#endif 

#include  <stdio.h> 
#include  <math.h> 


/*  manifest  constants  */ 
#define  FAILURE  -1 

#define  PI  3.1415926536 
#def ine  SQR(x)  x*x 
#def ine  QUB(x)  x*x*x 

FILE  *f 1; 

/*  typedefs  */ 
typedef  struct  point  { 
double  x,y; 

>  Point; 

typedef  struct  vertex  { 
Point  point; 
struct  vertex 
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♦previous, 
♦next ; 

}  Vertex; 


typedef  struct  { 

Point  point; 
double  theta; 
double  kappa; 

>C0NFIGURATI0N  ; 

typedef  struct  polygon  { 
int  degree; 

Vertex  ♦vertex.list ;  /♦  this  is  the  first  vertex  ♦/ 

struct  polygon 
♦previous, 

♦next ; 

>  Polygon; 

typedef  struct  world  { 
int  degree; 

Polygon  ♦poly_list ; 

}  World; 


/*************♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦***♦**♦*********** 
Function  name  :  OpenFile 

Purpose  :  This  function  is  used  to  open  a  file 

Parameters  :  - 

Return  Type  :  - 

*************************************************♦♦♦♦♦♦/ 


void  OpenFile () 

{ 

fl=  fopen("poly5 .dat","w") ; 

> 


/************************♦♦♦♦♦♦♦♦♦♦♦♦♦♦**♦******♦*****♦********************* 
Function  :  fatal () 

Parameters:  char  *message 
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Purpose  :  on  fatal  error  print  message  then  print  system  message  then  exit 
Called  by  :  create_world()  <world.c> 
create_polygon()  <world . c> 
add_vertex_to_polygon()  <world.c> 

Calls  :  NONE 

* *$*$** *********************************************************** **********  / 

void  fatal (message) 
char  *message; 

{ 

fprintf (stderr,  "Fatal  error  occured:  "); 
perror (message) ; 
exit (FAILURE); 

> 


Function 

Purpose 

Returns 

Called  by 

Calls 

Comments 


create_world() 

create  instance  of  a  world 

World  * 

ANYBODY 

fatal ()  futilities. c> 

this  function  allocates  space  for  a  world  and  returns  a  pointer 


********  ***************:!:%*%*******************************  ******************/ 


World  *create_world() 

{ 

World  *w; 

/*  allocate  memory  for  a  world  */ 

if((w  =  (World  *)malloc(sizeof (World)))  ==  NULL)  { 

fatal ("create_world:  malloc\n") ; 
exit (FAILURE) ; 

> 

/*  initialize  fields  */ 
w->degree  =  0; 
w->poly_list  =  NULL; 

return (w) ; 

> 
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/jit*********************************************************************** 

Function  name  :  Composition 

Purpose  :  This  function  is  used  to  compute  the  composition 

of  two  given  transformations 

Parameters  :  two  two  dimensional  coordinate  transformations 

Return  Type  :  CONFIGURATION-the  composition  of  the  two 

transformations  ql  and  q2 

sis***************************#***************#***************************/ 

CONFIGURATION  composition (ql ,q2) 

CONFIGURATION  ql; 

CONFIGURATION  q2; 

{ 

CONFIGURATION  q3; 

q3. point. x  =  ql. point. x  +  q2. point. x  *  cos(ql .theta)  - 
q2. point. y  *  sin (ql .theta) ; 

q3. point. y  =  ql. point. y  +  q2. point. x  *  sin(ql .theta)  + 
q2. point. y  *  cos (ql .theta) ; 
q3. theta  =  ql. theta  +  q2. theta; 


return  q3; 


/***************************************************************************** 
Function  name  :  Get_Circ_Transf ormation 

Purpose  :  This  function  is  used  to  compute  the  circular  transformation 

Parameters  :  fcdelta.theta  ,  &delta_s 

Return  Type  :  CONFIGURATION 

*****************************************************************************/ 

CONFIGURATION  Get.Circular.Transf ormation(delta_s ,delta_theta) 
double  delta_s,delta_theta; 

{ 

CONFIGURATION  q; 

q. point. x  —  (1.0  -  ( (delta_theta  *  delta_theta)/6.0)  + 

( (delta_theta*delta_theta*delta_theta*delta_theta)/120 . 0) ) 

*  delta_s; 

q. point. y  =  (0.5  -  ( (delta. theta  *  delta_theta)/24.0)  + 

( (delt a_theta*delta_theta*delta_theta*delta_theta) /720 . 0) ) 
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> 


*  delta_theta  *  delta_s; 
q. theta  =  delta.theta; 

return  (q) ; 


/***************************************************************************** 
Function  name  :  NormalizeO 

Purpose  :  This  function  is  used  to  get  the  value  of  sigma  from  the  user 

Parameters  :  - 

Return  Type  :  double 

*****************************************************************************/ 


double  Normalize (angle) 
double  angle; 

{ 

while  (  angle  >  PI  ) 

{ 

angle  -=  2*PI; 

> 

while  (  angle  <  -PI) 

{ 

angle  +=  2*PI ; 

} 

return  angle; 

> 


/******************************************************************** 
Function  name  :  steer 0 

Purpose  :  computes  steering  function 

Parameters  :  double  a, double  b  , double  c  , 

CONFIGURATION  ql .CONFIGURATION  q2 


Return  Type  :  double 

*********************************************************************/ 


double  steer(a,b,c,ql,q2) 
double  a,b,c; 
CONFIGURATION  ql,q2; 
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return  (-  (a  *  ql. kappa  +  b  *Normalize(ql .theta  -  q2.th.eta)  + 
c  *(-(ql. point .x-q2. point .x) *sin(q2. theta) + 

(ql . point . y-q2 . point . y) *cos (q2 . theta) )  )  ) ; 


> 


/******************************************************************** 
Function  name  :  Get .constants () 

Purpose  :  calculates  the  value  of  the  constants  a.b.c 

Parameters  :  - 

Return  Type  :  - 

*********************************************************************/ 

void  Get_constants(sigma,a,b,c) 
double  *a,*b,*c, sigma; 

i 

double  k; 

printf  ("\nEnter  the  value  of  sigma  please  : ")  ;  scanf  ("'/.If "  ,&sigma)  ; 

k  =  1.0/sigma; 

*a  =  3  *  k; 

*b  =  3  *  SQR(k); 

*c  =  QUB(k); 

} 

/******************************************************************** 
Function  name  :  move_next_step() 

Purpose  :  apply  steering  function  ,  get  next  configuration 

Parameters  :  double  s, double  sigma  , double  delta.s  , 

double  delta. theta  .CONFIGURATION  ql .CONFIGURATION  q2) 
Return  Type  :  CONFIGURATION 

*********************************************************************/ 


CONFIGURATION  move_next_step(a,b,c,delta_s  ,ql,q2) 
double  delta_s,a,b,c; 

CONFIGURATION  ql , q2 ; 

CONFIGURATION  circ; 

doubTe  lamda.delta.theta, sigma; 
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lamda  =  -  (a  *  ql. kappa  +  b  *Normalize(ql .theta  -  q2. theta)  + 
c  * (-(ql .point .x-q2. point .x)*sin(q2 .theta) + 

(ql . point . y-q2 . point . y) *cos (q2 . theta) ) ) ; 

ql. kappa  =  ql. kappa  +  lamda  *  delta_s; 
delta_theta  =  ql. kappa  *  delta_s; 

circ  =  Get_Circular_Transformation(delta_s,delta_theta) ; 

ql. point. x  =  ql. point. x  +  circ. point. x  *  cos(ql. theta)  - 
circ. point. y  *  sin(ql .theta) ; 
ql. point. y  =  ql. point. y  +  circ. point. x  *  sin(ql .theta)  + 
circ. point. y  *  cos (ql .theta) ; 
ql. theta  =  ql. theta  +  circ. theta; 

return  (ql) ;  - 1  *  ... 

> 


/ ************************************************************************** ** 


Function 

Purpose 

Returns 

Called  by 

Calls 

Comments 


creat e.polygon ( ) 

create  instance  of  a  polygon 

Polygon  * 

ANYBODY 

fatalO  <utilities .c> 

this  function  allocates  space  for  a  polygon  and  returns  a  pointer 


to  it 

****************************************************************************/ 


Polygon  *creat e.polygon () 

Polygon  *p; 

/*  allocate  memory  for  a  polygon  */ 

if((p  =  (Polygon  *)malloc(sizeof (Polygon)) )  ==  NULL)  { 

fatal ("creat e.polygon:  malloc\n") ; 
exit (FAILURE) ; 

} 


/*  initialize  fields  */ 
p->degree  =  0; 
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p->vertex_list  =  NULL; 
p->previous  =  NULL; 
p->next  =  NULL; 

return (p) ; 

> 


/*,m<************************************************************************ 


Function  : 
Parameters: 


add_vertex_to_polygon(x,  y,  p) 
double  x  x  coordinate  of  new  vertex 


double  y  y  coordinate  of  new  vertex 
Polygon  *p  pointer  to  an  existing  polygon 
Purpose  :  add  a  vertex  to  an  existing  polygon 

Returns  :  void 

Called  by  :  ANYBODY 

Calls  :  fatal O  <utilities . c> 

Comments  :  adds  vertex  to  the  end  of  the  vertex  list  -  no  way  to  insert 

NOTE:  polygon  must  exist  before  adding  vertices 
****************************************************************************/ 


void  add_vertex_to_polygon(x,  y,  p) 
double  x,  y; 

Polygon  *p; 

{ 

int  i;  /*  loop  variable  */ 

Vertex 

*new_vertex,  /*  pointer  to  the  new  vertex  */ 

♦current .vertex;  /*  pointer  to  the  current  vertex  */ 

/♦  allocate  space  for  the  new  vertex  ♦/ 

if ( (new.vertex  =  (Vertex  *)malloc(sizeof (Vertex)))  ==  NULL)  { 
fatal("add_vertex_to_polygon:  malloc\n") ; 
exit (FAILURE) ; 

} 

/*  install  coordinates  */ 
new_vertex->point .x  =  x; 
new_vertex->point .y  =  y; 


/*  check  if  first  vertex  */ 
if(p->degree  ==  0)  { 
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new_vertex->previous  =  new.vertex; 
p->vertex_list  =  new.vertex; 

} 

else  { 

/*  set  up  the  links  */ 

current .vertex  =  p->vertex_list; 

current .vertex- >previous  =  new. vertex; 

/*  find  the  last  vertex  */ 
for(i  =  1;  i  <  p->degree;  i++) 

current .vertex  =  current _vertex->next; 

new_vertex->previous  =  cur rent .vert ex; 
current .vert ex- >next  =  new.vertex; 


> 

new_vertex->next  =  p->vertex_list ; 
p->degree++; 

> 


/**********************:Ms=Mt**************************************************** 


Function 
Parameters 
Purpose 
Returns 
Called  by 
Calls 


display_vertices_of_polygon(p) 

Polygon  *p  pointer  to  an  existing  polygon 

display  the  vertices  of  a  polygon  of  the  existing  world 

void 

ANYBODY 

NONE 


Comments 

******************************************************************************/ 


void  display.vert ices.of .polygon (p) 
Polygon  *p; 


Vertex  *current,  *first; 

f irst=current=  p->vertex_list ; 
do{ 

printf  ("\n*/.f  */.f\n",current->point  .x  ,  current->point  .y)  ; 
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current  =  current ->next ; 
}while (current  !=  first); 
return ; 


Function  : 
Parameters: 

Purpose 
Returns 
Galled  by 
Calls 
Comments 


add_polygon_to_world(p,  w) 

Polygon  *p  pointer  to  an  existing  polygon 

World  *w  pointer  to  an  existing  world 

add  an  existing  polygon  to  an  existing  world 

void 

ANYBODY 

NONE 

adds  polygon  to  end  of  polygon  list  -  no  way  to  insert  polygons 


void  add_polygon_to_world(p,  w) 

Polygon  *p; 

World  *w; 

{ 

Polygon  ^current .polygon;  /*  pointer  to  current  polygon  */ 

/*  check  if  first  polygon  */ 
if(w->degree  ==  0)  { 
w->poly_list  =  p; 

> 

else  { 

/*  find  the  last  polygon  */ 
current .polygon  =  w->poly_list ; 
while (current _polygon->next  !=  NULL) 

current .polygon  =  current _polygon->next; 

p->previous  =  current .polygon; 
current _polygon->next  =  p; 

> 


w->degree++; 

> 
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/^^^^*^^ **************************************************** ************** 
Function  :  Find_Path_Segment(v) 

Parameters:  vertex 

Purpose  :  computes  the  path  segment  according  to  a  given  safety  distance 

Returns  :  CONFIGURATION 

Called  by  :  ANYBODY 

Calls  :  NONE 

Comments  :  safety  distance  is  -50  by  default (-  shows  ccw  polygon, +  cw  ) 

jit*************************************************************************/ 


CONFIGURATION  Find_Path_Segment (v) 
Vertex  *v; 

{ 

CONFIGURATION  safe; 

CONFIGURATION  edge; 


safe . point . x=0 . 0 ; 

safe .point .y=-50 . 0 ; 

safe . theta=0 . 0 ; 

safe.kappa=0.0; 

edge .point ,x=v->point .x; 

edge . point . y=v->point . y ; 

edge .theta=  atan2(v->next->point .y  -  v  ->point.y, 

v->next->point .x  -  v->point.x); 


edge . kappa=0 . 0 ; 


return(composition(edge,saf e) ) ; 

> 


/******************************************* 
Function:  mainO 

Purpose  :  this  is  the  main  function 

*******************************************/ 

int  mainO 

{ 


Vertex  *new_vertex; 
Polygon  *pl; 

World  *w; 
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Vertex  *v,*first; 
double  a,b,c,k; 

double  delt a_s , delta. thet a , sigma , lamda; 
int  i=0; 
int  N=4 ; 

CONFIGURATION  e,q[6] ,qo,qv; 

/*  make  a  world  */ 
w  =  create_world() ; 


/*  make  a  polygon  */ 
pi  =  create.polygonO  ; 


/*  add  three  vertices  */ 
add. vert ex.t o.polygon (153.2, 
add_vertex_to_polygon(397 .2, 
add. vert ex.t o.polygon (397 .2, 
add. vert  ex.t o.polygon (153.2, 


118.0,  pi); 
118.0,  pi) ; 
182.0,  pi); 
182.0,  pi); 


display.vertices.of .polygon (pi) ; 

/*  attach  polygon  to  world  */ 
add_polygon_to_world(pl ,  w) ; 


OpenFileQ ; 


v=first  =  pl->vertex_list ; 
do{ 

q[i]  =  Find.Path.Segment(v) ; 

v  =  v->next; 

i++; 

}while(  v  !=  first); 


q[N]  =q[0]  ; 
q[N+l]  =q[l]  ; 

/*  qv  is  the  actual  point  we’re  on  */ 

qv=q[0]  ; ^initialization  of  the  vehicle  configuration*/ 

printf  ("\nEnter  the  value  of  delta.s  please  : ")  ;scanf  ("'/,lf "  ,&delta_s) 

Get .const ants (sigma, &a,&b,&c) ; 
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i=0; 

do 

{ 

qv  =  move_next_step(a,b,c,delta_s,qv,q[i]) ; 

fprintf  (fl,"\n‘/.15.10f  */,15 .  lOf"  ,qv. point  .x  , qv. point .y) ; 

if  (steer(a,b,c,qv,q[i+l] )>=  0.0) 

i++;/*  switching  lines  */ 

y 

}while(i<=N) ; 

fclose(f 1) ; 
return  0; 


#if  0 


Thesis  Research 


Filename 

Author 

Operating  System 
Description 


star  .c 

Karamanlis  Vasilios 
Unix 

This  file  contains  all  the  necessary  functions  to 
track  a  star  using  the  new  method  "neutral  switching" 


#endif 

#include  <stdio.h> 
#include  <math.h> 

#def ine  PI  3.14159265 
#define  NUM  4  *PI/5 
#def ine  SQR(x)  x*x 
#def ine  QUB(x)  x*x*x 

FILE  *fl; 


typedef  struct  { 
double  x; 
double  y; 
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double  theta; 
double  kappa; 

3-CONFIGURATION ; 

/************************************************************************ 
Function  name  :  write_to_f ile() 

Purpose  :  This  function  is  used  to  write  the  data  to  the 

output  file 
Parameters  :  two  doubles 

Return  Type 

************************************************************************/ 

void  write_to_f ile(a  ,b) 
double  a,b; 

{ 

f printf  (f  1 ,  "\n*/.10 . 5f  7.10 . 5f , a ,b)  ; 

} 


/************************************************************************ 
Function  name  :  Composition 

Purpose  :  This  function  is  used  to  compute  the  composition 

of  two  given  transf ormations 

Parameters  :  two  two  dimensional  coordinate  transformations 

Return  Type  :  CONFIGURATION-the  composition  of  the  two 

transformations  ql  and  q2 

************************************************************************/ 

CONFIGURATION 
compose (first , second) 

CONFIGURATION  *  first; 

CONFIGURATION  *  second; 

{ 

CONFIGURATION  third; 
double  x,y,  theta; 
double  xx,yy,tt; 

x  =  second->x; 
y  =  second->y; 
theta  =  f irst->theta; 
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XX  =  cos (theta)  *  x  -  sin (theta)  *  y  +  first->x; 
yy  =  sin (theta)  *  x  +  cos (theta)  *  y  +  first _>y; 

tt  =  first->theta  +  second->theta; 

third. x  =  xx; 
third. y  =  yy; 
third,  theta  =  tt; 
third. kappa  =  0.0; 

return  third; 

} 


/***************************************************************************** 
Function  name  :  Normalize () 

Purpose  :  This  function  is  used  to  get  the  value  of  sigma  from  the  user 

Parameters  :  - 

Return  Type  :  double 

*****************************************************************************/ 

double  Normalize (angle) 
double  angle; 

while  (  angle  >  PI  ) 

angle  -■  2*PI ; 

> 

while  (  angle  <  -PI) 

{ 

angle  +=  2*PI ; 

> 

return  angle; 

> 


/***************************************************************************** 


Function  name 

Purpose 

Parameters 


Get_Circ_Transf ormation 

This  function  is  used  to  compute  the  circular  transformation 
&delta_theta  ,  &delta_s 
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Return  Type  :  CONFIGURATION 

*****************************************************************************/ 


CONFIGURATION  Get.Circular.Transf ormation(delta_s ,delta_theta) 
double  delta.s ,delta_theta; 

{ 

CONFIGURATION  q; 

double  delta_theta2 ,delta_theta4; 
delta_theta2=delta_theta*delta_theta; 

delta_theta4=delta_theta*delta_theta*delta_theta*delta_ theta; 

q.x  =  (1.0  -  ( (delta_theta2) /6 . 0)  +  ( (delta, theta4)/ 120.0))  *  delta.s; 
q.y  =  (0.5  -  ((delta_theta2)/24.0)  +  ((delta_theta4)/720.0)) 

*  delta.theta  *  delta.s; 
q. theta  =  delta.theta; 

return  (q) ; 

> 

/******************************************************************** 
Function  name  :  steer () 

Purpose  :  computes  steering  function 

Parameters  :  double  a, double  b  , double  c  , 

CONFIGURATION  ql .CONFIGURATION  q2 
Return  Type  :  double 

*********************************************************************/ 


double  steer(a,b,c,ql,q2) 
double  a,b,c; 

CONFIGURATION  ql,q2; 

return  (—  (a  *  ql. kappa  +  b  *Normalize(ql .theta  —  q2. theta)  + 

c  * (- (ql - x-q2 . x) *s in (q2 . thet a)  +  (ql . y-q2 . y ) *co s (q2 . theta) ) ) ) ; 

> 


/******* ************************************************************* 
Function  name  :  move.next.stepO 
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Purpose  :  apply  steering  function  ,  get  next  configuration 

Parameters  :  double  s. double  sigma  , double  delta_s  , 

double  delta.theta  .CONFIGURATION  ql, CONFIGURATION  q2) 
Return  Type  :  CONFIGURATION 

*********************************************************************/ 

CONFIGURATION  move_next_step(a,b, c ,delta_s  ,ql,q2) 
double  delta_s,a,b,c; 

CONFIGURATION  ql,q2; 

{ 

CONFIGURATION  circ; 

double  lamda,delta_theta, sigma; 

lamda=steer(a,b,c,ql,q2) ; 

ql. kappa  *  ql. kappa  +  lamda  *  delta.s; 
delta_theta  =  ql. kappa  *  delta.s; 

circ  =  Get_Circular_Transformation(delta_s,delta_theta) ; 
ql=compose(ql , circ) ; 

return  (ql); 

> 

/******************************************************************** 
Function  name  :  Get .const ant s() 

Purpose  :  calculates  the  value  of  the  constants  a,b,c 

Parameters  :  - 

Return  Type  :  - 

********* ***************************************** ******************* / 
void  Get_constants(sigma,a,b,c) 
double  *a,*b,*c, sigma; 

{ 

double  k; 

printf  ("\nEnter  the  value  of  sigma  please  : ")  ;scanf  ("*/,lf "  .ftsigma)  ; 

k  =  1.0/sigma; 

*a  =  3  *  k; 

*b  =  3  *  SQR(k) ; 

*c  =  QUB(k) ; 

> 
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/*************************************************************************** 
Function  :  def  ineConf  igO 

Parameters  :  double  x,y, theta, kappa  — The  values  that  define  a 

configuration 

Purpose  :  To  allocate  nad  assign  a  configuration 

Returns  :  CONFIGURATION:  a  configuration 

Comments  :  Was  called  def _conf igurationO  in  MML10 

******************************************************************************/ 

CONFIGURATION  def ineConf ig(x,y, theta, kappa) 
double  x,y, theta, kappa; 

{ 

CONFIGURATION  newConfig; 

newConfig.x  =  x; 
newConfig. y  =  y; 
newConfig. theta  =  theta; 
newConfig. kappa  =  kappa; 

return  newConfig; 

> 


/****************************************** 
Function  :  main() 

Purpose  :  this  is  the  main  function 

*******************************************/ 

int  main() 

{ 

int  i; 

double  a,b,c,k; 

double  delta_s,delta_theta, sigma; 

CONFIGURATION  turn, q [6] ,qv; 

q[0]=def ineConf ig(0. 0,0. 0,0. 0,0.0) ; 
turn=def ineConf ig (300.0,0.0 ,NUM, 0 . 0) ; 

f l=fopen("starl0.dat","w") ; 
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for(i=0;  i  <=5;  i++) 

{ 

q[i+l]=compose(q[i] ,turn) ; 
write_to_f ile(q[i+l] .x,q[i+l] .y) ; 

> 

/*  qv  is  the  actual  point  we’re  on  */ 

qv=q[0] ;/*initialization  of  the  vehicle  configuration*/ 

printf ("\nEnter  the  value  of  delta.s  please  :") ;scanf ("5#lf " ,&delta_s) ; 

Get_constants(sigma,&a,&b,&c) ; 

i=0; 

do 

qv  =  move_next_step(a,b,c,delta_s,qv,q[i] ) ; 
f printf  (fl,"\n*/.15.  lOf  ’/.15 . lOf  " ,qv.x  ,qv.y); 

if  (steer(a,b,c,qv,q[i+l] )>=  0.0) 

{ 

i++;/*  switching  lines  */ 

> 

}while(i<=5) ; 

fclose(f 1) ; 
return  (0) ; 
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